CylindricDepthTrackingPanorama.cpp

#include <Image/CylindricDepthTrackingPanorama.hh>
#include <Base/Image/ImageIO.hh>
#include <Image/MixtureOfGaussians.hh>
#include <Base/ImageUtils/ImageDraw.hh>
#include <Image/CylindricDepthPanorama.hh>
#include <Base/Common/FileHandling.hh>

using namespace std;

namespace BIAS
{

CylindricDepthTrackingPanorama::
CylindricDepthTrackingPanorama(const Projection& initialProjection, 
                               unsigned int channels, 
                               float depthScale)
: CylindricDepthPanorama(initialProjection, channels) {
  dTexChannels_ = channels;
  fDepthScale_ = depthScale;
  panoramaNonStatic_.Init(panoramaWidth_,panoramaHeight_,1);
  panoramaNonStaticTexture_.Init(panoramaWidth_,panoramaHeight_,dTexChannels_);
  mog_ = new MixtureOfGaussians<float>(5, 3, 0.2f );
  mog_->InitWeights(100, 20, 2.9f );
  mog_->SetSmoothing(true);
  bInited_ = false;
}

CylindricDepthTrackingPanorama::~CylindricDepthTrackingPanorama()
{
}

int  CylindricDepthTrackingPanorama::
ProcessCylindricProjection(BIAS::Image<float> &depthMap,
                           BIAS::Projection & p,
                           BIAS::Image<unsigned char> *texture)
{
 if(!bFirstPoseSet_){
    initialProjection_.GetParameters()->
      SetPose(p.GetParameters()->GetPose());    
    //contruct cylindrical projection    
    //obsolete, this is a pointer to the above anyway
    pCylinderProj_->SetPose(p.GetParameters()->GetPose());
    bFirstPoseSet_ = true;
  }

  int ret = 0;
  unsigned int width = depthMap.GetWidth();
  unsigned int height = depthMap.GetHeight();

  //float depthValue=0.0; not used anywhere

  normalizedDepthMap_ = depthMap;

  float **ida = depthMap.GetImageDataArray();
  float **idaN = normalizedDepthMap_.GetImageDataArray();

  //determine bounding box

  HomgPoint2D min, max;
  min[0] = width; min[1] = height;
  max[0] = 0; max[1] = 0;
  bool found = false;
  for(unsigned int y=0;y<height;y++){
    for(unsigned int x=0;x<width;x++){
      if (ida[y][x] != 0.0 && ida[y][x] != 1.0) {
        min[1] = y;
        min[0] = x;
        found = true;
        break;
      }
    }
    if (found) break;
  }

  found = false;
  for(unsigned int y=height-1;y>min[1];y--){
    for(unsigned int x=width-1;x>0;x--){
      if (ida[y][x] != 0.0 && ida[y][x] != 1.0) {
        max[1] = y;
        max[0] = x;
        found = true;
        break;
      }
    }
    if (found) break;
  }

  for(unsigned int y=(unsigned int)min[1];y<(unsigned int)max[1];y++){
    for(unsigned int x=(unsigned int)min[0];x>0;x--){
      if (ida[y][x] != 0.0 && ida[y][x] != 1.0) {
        min[0] = x;
      } else {
        break;
      }
    }
  }

  for(unsigned int y=(unsigned int)min[1];y<(unsigned int)max[1];y++){
    for(unsigned int x=(unsigned int)max[0];x<width;x++){
      if (ida[y][x] != 0.0 && ida[y][x] != 1.0) {
        max[0] = x;
      } else {
        break;
      }
    }
  }

  // roi to 2/3 of image
  // cut 1/6 of each side
  unsigned int xdiff = (unsigned int)rint(max[0]-min[0]) / 6;
  unsigned int ydiff = (unsigned int)rint(max[1]-min[1]) / 6;
  // roi to 1/2 of image
  // cut 1/4 of each side
/*  unsigned int xdiff = (max[0]-min[0]) / 4;
  unsigned int ydiff = (max[1]-min[1]) / 4;
*/
  min[0] += xdiff;
  min[1] += ydiff;
  max[0] -= xdiff;
  max[1] -= ydiff;
  for(unsigned int x=(unsigned int)min[0];x<(unsigned int)max[0];x++){
    for(unsigned int y=(unsigned int)min[1];y<(unsigned int)max[1];y++){
      if (ida[y][x] == 1.0) {
        ida[y][x] = 0.0;
        idaN[y][x] = 0.0;
        continue;
      }
      ida[y][x] *= fDepthScale_;
    }
  }
  cout<<"determined roi:"<<min[0]<<","<< min[1]<<","<< max[0]<<","<< max[1]<<endl;

  depthMap.SetROICorners((unsigned int)min[0], (unsigned int)min[1],
                         (unsigned int)max[0], (unsigned int)max[1]); //   if (time_ != -1 && track_) {
  normalizedDepthMap_.SetROICorners((unsigned int)min[0], (unsigned int)min[1],
                          (unsigned int)max[0],(unsigned int) max[1]);
  ret = doTrack(depthMap, p, texture);

 // mog_->GetDifferenceImageWithVisuals(visualisation_);
/* DEBUG
  static int n=0;
  if (!visualisation_.IsEmpty())
    ImageIO::Save("MOGVisu"+LeadingZeroString(n++,3)+".mip", visualisation_);
*/
  return ret;
}

int CylindricDepthTrackingPanorama::LoadPanorama(const std::string filename) {
  int ret = 0;
  ret += ImageIO::Load(filename+"-TrackingPanoramaDepth.mip", panorama_);
  ret += ImageIO::Load(filename+"-TrackingPanoramaTexture.mip", panoramaTexture_);
  ret += ImageIO::Load(filename+"-TrackingPanoramaHitCount.mip", panoramaHitCount_);
  cout << " LOADING PANORAMA" << endl;
  for (int i=0;i<50;i++) {
   // cout << "doone." << endl;
    mog_->Apply(panorama_);
  }
  bTextureFilled_ = true;
  return ret;
}

int CylindricDepthTrackingPanorama::SavePanorama(const std::string filename) {
  int ret = 0;
  ret += ImageIO::Save(filename+"-TrackingPanoramaDepth.mip", panorama_);
  ret += ImageIO::Save(filename+"-TrackingPanoramaTexture.mip", panoramaTexture_);
  ret += ImageIO::Save(filename+"-TrackingPanoramaHitCount.mip", panoramaHitCount_);
//  cout << "setting up motion detection (might take a while)" << endl;
//  if (!bInited_) {
//    for (int i=0;i<50;i++) {
//      mog_->Apply(panorama_);
//    }
//  }
//  cout << "setting up motion detection complete" << endl;
  bInited_ = true;
  return ret;
}

int CylindricDepthTrackingPanorama::
GetTriangleMesh(BIAS::TriangleMesh &mesh){
  int ret =-1;
  BIAS::Image<unsigned char> tmpUC;
  if(bTextureFilled_) {
    int factor = 4;
    BIAS::Projection pRescaledProjection_(initialProjection_);
    pRescaledProjection_.Rescale((float)factor);
    BIAS::Image<float> depthDownSampled_(panoramaWidth_ / factor, panoramaHeight_ / factor);
    Rescale<float, float> pRescaleFilter_;
    pRescaleFilter_.SetFactor((float)factor);
      pRescaleFilter_.SetLowPassType(LPT_Gauss);
    pRescaleFilter_.Filter(panorama_, depthDownSampled_);
    int width = depthDownSampled_.GetWidth();
    int height = depthDownSampled_.GetHeight();
    // remove low depth values resulting from downsampling on 0.0 edges
    BIAS::Image<float> filteredDepthMap_(width, height);
    filteredDepthMap_.FillImageWithConstValue(0.0);

    // anti bubblegum: delete points of high variance
    float **idaF = filteredDepthMap_.GetImageDataArray();
    float **idaD = depthDownSampled_.GetImageDataArray();
    for (int x=0;x<width;x++) {
      for (int y=0;y<height;y++) {
        if (idaD[y][x] < 10.0)
          continue;
        float minDepth_ = idaD[y][x];
        float maxDepth_ = idaD[y][x];
        for (int xw=x-1;xw<=x+1;xw+=1) {
          for (int yw=y-1;yw<=y+1;yw+=1) {
            if (xw >= 0 && yw >= 0 && xw < width && yw < height) {
              if (idaD[yw][xw] > maxDepth_)
                maxDepth_ = idaD[yw][xw];
              if (idaD[yw][xw] < minDepth_)
                minDepth_ = idaD[yw][xw];
            }
          }
        }
        if (maxDepth_ - minDepth_ < 1000.0)
          idaF[y][x] = idaD[y][x];
      }
    }
    depthDownSampled_ = filteredDepthMap_;

    cout << initialProjection_ << endl;
    cout << pRescaledProjection_ << endl;
    BIAS::ImageIO::Save("Panorama_DepthMap.mip", panorama_);
    BIAS::ImageIO::Save("Panorama_DepthMap_Downsampled.mip", depthDownSampled_);
    BIAS::ImageIO::Save("Panorama_Texture.mip", panoramaTexture_);
    mesh.GenerateDenseMesh(depthDownSampled_,pRescaledProjection_,
                           panoramaTexture_, initialProjection_);
    //pInitialProjection_->Rescale((float)1.0/float(factor));

    static int n=0;
    SavePanorama(FileHandling::LeadingZeroString(n++,3));
    return 0;
  }
  return ret;
}

int  CylindricDepthTrackingPanorama::
doPanorama(BIAS::Image<float> &depthMap,
           BIAS::Projection & p,
           BIAS::Image<unsigned char> *texture)
{
  int ret =0;
  BIAS::Vector4<unsigned> tmpCorners;
  depthMap.GetROICorners(tmpCorners[0],tmpCorners[1],tmpCorners[2],tmpCorners[3]);

  bb_.UL[0] = (unsigned)tmpCorners[0];
  bb_.UL[1] = (unsigned)tmpCorners[1];
  bb_.LR[0] = (unsigned)tmpCorners[2];
  bb_.LR[1] = (unsigned)tmpCorners[3];

  cout << " DO PANORAMA" << endl;
  float **ida = depthMap.GetImageDataArray();
  float **idaP = panorama_.GetImageDataArray();
  float **idaPHC = panoramaHitCount_.GetImageDataArray();
  unsigned char ** idaTex = panoramaTexture_.GetImageDataArray();

  //unsigned int width = depthMap.GetWidth();
  //unsigned int height = depthMap.GetHeight();

  float depthValue=0.0;

  for (unsigned int y=(unsigned)bb_.UL[1];y<(unsigned)bb_.LR[1]-1;y++) {
    for (unsigned int x=(unsigned)bb_.UL[0];x<(unsigned)bb_.LR[0]-1;x++) {
      depthValue = ida[y][x];
      if (depthValue > 0.0) {
        //fill texture panorama with values
        if(texture != NULL){
          for (unsigned int i=0;i<dTexChannels_;i++)
            idaTex[y][x*dTexChannels_+i] = texture->GetImageDataArray()[y][x*dTexChannels_+i];
          bTextureFilled_ = true;
        }
        if(idaPHC[y][x] == 0.0){
          idaP[y][x]=depthValue;
          idaPHC[y][x]=1.0;
        }
        else{
          idaP[y][x]=
            (idaP[y][x] * idaPHC[y][x]+depthValue)/(idaPHC[y][x]+1);
          idaPHC[y][x]+=1.0;
        }
      }
    }
  }

  return ret;
}

int  CylindricDepthTrackingPanorama::
doTrack(BIAS::Image<float> &depthMap,
                           BIAS::Projection & p,
                           BIAS::Image<unsigned char> *texture)
{
  mog_->Apply(normalizedDepthMap_);
  mog_->GetDifferenceImage(difference_);
  //mog_->GetLargestBoundingBox(bb);
  //
  cout << "TRACK!" << endl;
  BIAS::Vector4<unsigned> tmpCorners;
  depthMap.GetROICorners(tmpCorners[0],tmpCorners[1],tmpCorners[2],tmpCorners[3]);

  bb_.UL[0] = (unsigned)tmpCorners[0];
  bb_.UL[1] = (unsigned)tmpCorners[1];
  bb_.LR[0] = (unsigned)tmpCorners[2];
  bb_.LR[1] = (unsigned)tmpCorners[3];

  float **ida = depthMap.GetImageDataArray();
  unsigned char **idaT = texture->GetImageDataArray();
  unsigned char **idaDif = difference_.GetImageDataArray();
  float **idaNsDep = panoramaNonStatic_.GetImageDataArray();
  unsigned char **idaNsTex = panoramaNonStaticTexture_.GetImageDataArray();
  float **idaD = panorama_.GetImageDataArray();
  unsigned char ** idaTex = panoramaTexture_.GetImageDataArray();
  float **idaPHC = panoramaHitCount_.GetImageDataArray();

  for (unsigned int y=(unsigned)bb_.UL[1];y<(unsigned)bb_.LR[1]-1;y++) {
    for (unsigned int x=(unsigned)bb_.UL[0];x<(unsigned)bb_.LR[0]-1;x++) {
      float depthValue = ida[y][x];
      if (idaD[y][x]==0) {
        if (depthValue > 0.0) {
          //fill texture panorama with values
          if(texture != NULL){
            for (unsigned int i=0;i<dTexChannels_;i++)
              idaTex[y][x*dTexChannels_+i] = idaT[y][x*dTexChannels_+i];
            bTextureFilled_ = true;
          }
          if(idaPHC[y][x] == 0.0){
            idaD[y][x]=depthValue;
            idaPHC[y][x]=1.0;
          }
          else{
            idaD[y][x]=
              (idaD[y][x] * idaPHC[y][x]+depthValue)/(idaPHC[y][x]+1);
            idaPHC[y][x]+=1.0;
          }
        }
      } else {
        if (idaDif[y][x] > 64) {
          if (abs(depthValue - idaD[y][x]) > 400.0) {
            idaNsDep[y][x] = depthValue;
            for (unsigned int i=0;i<dTexChannels_;i++)
              idaNsTex[y][x*dTexChannels_+i] = idaT[y][x*dTexChannels_+i];
          } else {
            for (unsigned int i=0;i<dTexChannels_;i++) {
             // idaTex[y][x*dTexChannels_+i] = (idaTex[y][x*dTexChannels_+i] + idaT[y][x*dTexChannels_+i]) / 2; // TODO
              idaTex[y][x*dTexChannels_+i] = idaT[y][x*dTexChannels_+i];
              idaNsTex[y][x*dTexChannels_+i] = 0;
            }
            idaNsDep[y][x] = 0;
            idaD[y][x]=
              (idaD[y][x] * idaPHC[y][x]+depthValue)/(idaPHC[y][x]+1);
            idaPHC[y][x]+=1.0;

          }
        }
        else {
          idaNsDep[y][x] = 0;
          for (unsigned int i=0;i<dTexChannels_;i++)
            idaNsTex[y][x*dTexChannels_+i] = 0;
          idaD[y][x]=
            (idaD[y][x] * idaPHC[y][x]+depthValue)/(idaPHC[y][x]+1);
          idaPHC[y][x]+=1.0;
       }
      }
    }
  }
  //texture = &panoramaNonStaticTexture_;

  return 0;
}


int  CylindricDepthTrackingPanorama::
GetPanorama(BIAS::Image<float> &image,
            BIAS::Image<unsigned char> &texture){
  int ret = CylindricDepthPanorama::GetPanorama(image, texture);

  unsigned char **idaT = texture.GetImageDataArray();
  unsigned char **idaNsTex = panoramaNonStaticTexture_.GetImageDataArray();
  float **idaNsDep = panoramaNonStatic_.GetImageDataArray();
  BIAS::BIASBlob rectangle;
  rectangle.UL[0] = 0;
  rectangle.UL[1] = 0;
  rectangle.LR[0] = 0;
  rectangle.LR[1] = 0;
  bool draw = false;
  for (unsigned int y=(unsigned)bb_.UL[1];y<(unsigned)bb_.LR[1]-1;y++) {
    for (unsigned int x=(unsigned)bb_.UL[0];x<(unsigned)bb_.LR[0]-1;x++) {
      if (idaNsDep[y][x] != 0) {
        if (dTexChannels_ == 3) {
          idaT[y][x*dTexChannels_+0] = (unsigned char)(idaNsTex[y][x*dTexChannels_+0]*2);
          idaT[y][x*dTexChannels_+1] = 0;
          idaT[y][x*dTexChannels_+2] = idaNsTex[y][x*dTexChannels_+2];

        }
//        for (unsigned int i=0;i<dTexChannels_;i++)
  //        idaT[y][x*dTexChannels_+i] = idaNsTex[y][x*dTexChannels_+i];
        if (!draw) {
          draw = true;
          rectangle.UL[0] = x;
          rectangle.UL[1] = y;
          rectangle.LR[0] = x;
          rectangle.LR[1] = y;
        }
        if (draw) {
          if (x < rectangle.UL[0])
            rectangle.UL[0] = x;
          if (y < rectangle.UL[1])
            rectangle.UL[1] = y;
          if (x > rectangle.LR[0])
            rectangle.LR[0] = x;
          if (y > rectangle.LR[1])
            rectangle.LR[1] = y;
        }
      }
    }
  }
//  if (draw) {
//    ImageDraw<unsigned char>::RectangleCorners(texture, rectangle.UL.x,
//        rectangle.UL.y, rectangle.LR.x, rectangle.LR.y);
//  }
//  if (time_ == -1 || !track_) {
  return ret;
//  } else {
//  image = panorama_;
//  image.ScaleShiftBetween(0,255);
//  texture = panoramaNonStaticTexture_;
// DEBUG
//  static int n=0;
//  ImageIO::Save("MOGTexture"+LeadingZeroString(n,3)+".mip", panoramaNonStaticTexture_);
//  ImageIO::Save("MOGDepth"+LeadingZeroString(n++,3)+".mip", panoramaNonStatic_);

//  return ret;
//  }
}

} // end namespace BIAS