ForwardMappingNearestNeighbour.cpp

#include "ForwardMappingNearestNeighbour.hh"
#include <Base/Common/BIASpragma.hh>
#include <algorithm>

using namespace BIAS;
using namespace std;

template <class InputStorageType, class OutputStorageType>
ForwardMappingNearestNeighbour<InputStorageType, OutputStorageType>::
ForwardMappingNearestNeighbour() {
  fillblack_ = true;
  fillAreaWindowSize_=5;
}

template <class InputStorageType, class OutputStorageType>
ForwardMappingNearestNeighbour<InputStorageType, OutputStorageType>::
ForwardMappingNearestNeighbour(const ForwardMappingNearestNeighbour& src) {
  
}

template <class InputStorageType, class OutputStorageType>
ForwardMappingNearestNeighbour<InputStorageType, OutputStorageType>::
~ForwardMappingNearestNeighbour() {
  
}

template <class InputStorageType, class OutputStorageType>
ForwardMappingNearestNeighbour<InputStorageType, OutputStorageType>&
ForwardMappingNearestNeighbour<InputStorageType, OutputStorageType>::
operator=(const ForwardMappingNearestNeighbour<InputStorageType, OutputStorageType>& src) {
  SourceP_ = src.SourceP_;
  SinkP_ = src.SinkP_;
  intermediate_ = src.intermediate_;
  intermediateTex_ = src.intermediateTex_;
  dSourceWidth_ = src.dSourceWidth_;
  dSourceHeight_ = src.dSourceHeight_;
  dTargetWidth_ = src.dTargetWidth_;
  dTargetHeight_ = src.dTargetHeight_;

  return *this;
}

template <class InputStorageType, class OutputStorageType> 
int ForwardMappingNearestNeighbour<InputStorageType, OutputStorageType>::
Map(const BIAS::Image<InputStorageType> *in, BIAS::Image<OutputStorageType>& out,
    const BIAS::Image<InputStorageType> *texin, BIAS::Image<OutputStorageType>& texout) {
  bool withTex_= (texin == NULL) ? false : true;

  in->GetROI()->GetCorners(ulx_, uly_, lrx_, lry_);
  lrx_--; lry_--;
  if (out.IsEmpty()) {
    out.Init(dTargetWidth_, dTargetHeight_);
  }
  out.FillImageWithConstValue((OutputStorageType)0);
  intermediate_.FillImageWithConstValue((OutputStorageType)0);

  const InputStorageType**  idaSource_ = in->GetImageDataArray();
  OutputStorageType**       idaTarget_ = out.GetImageDataArray();
  const  InputStorageType** idaTexIn_ = NULL; 
  OutputStorageType**       idaTexOut_ = NULL; 
  OutputStorageType**       idaIntermediate_=intermediate_.GetImageDataArray();
  OutputStorageType**       idaIntermediateTex_=intermediateTex_.GetImageDataArray();
  if (withTex_) {
    idaTexIn_ = texin->GetImageDataArray();
    if (texout.IsEmpty())
      texout.Init(dTargetWidth_, dTargetHeight_, dTexChannels_);
    texout.FillImageWithConstValue((OutputStorageType)0);
    idaTexOut_ = texout.GetImageDataArray();
    intermediateTex_.FillImageWithConstValue((OutputStorageType)0);
  }

  //calculate projection of roi corners in cylindric image;
  int cornerpos[8] = { ulx_,uly_, lrx_,uly_, ulx_,lry_, lrx_,lry_ };
  HomgPoint2D corners[4];
  for (int i=0;i<4;i++) {
    HomgPoint2D p2d;
    p2d[0]=cornerpos[2*i];
    p2d[1]=cornerpos[2*i+1];
    p2d[2]=1;
    float fDepth = idaSource_[ cornerpos[2*i+1] ][ cornerpos[2*i] ];
    if (fDepth < 5.0) 
      fDepth = 1000.0;
    HomgPoint3D p3d = (BIAS::HomgPoint3D)SourceP_.UnProjectToPoint(p2d, fDepth);
    if (SinkP_.DoesPointProjectIntoImage(p3d, p2d)) {
    
      p2d.Homogenize();
      corners[i] = p2d;
    }
    else { 
  
      if (p2d[0] < 0)
        p2d[0] = 0;
      if (p2d[0] >= dTargetWidth_)
        p2d[0] = dTargetWidth_ -1;
      if (p2d[1] < 0)
        p2d[1] = 0;
      if (p2d[1] >= dTargetHeight_)
        p2d[1] = dTargetHeight_ -1;
      corners[i] = p2d;
    }
  }

  //check if all bounding corners are within panorama image and
  //sort bounding corners in min and max
  boundMin_[0]=dTargetWidth_; boundMin_[1]=dTargetHeight_; boundMin_[2] = 1;
  boundMax_[0]=0; boundMax_[1]=0; boundMax_[2] = 1;
  for (unsigned int j=0;j<2;j++) {
    for (unsigned int i=0;i<4;i++) {
      if (corners[i][j] < boundMin_[j]) {
        boundMin_[j] = corners[i][j];
      }
      if (corners[i][j]> boundMax_[j]) {
        boundMax_[j] = corners[i][j];
      }
    }
  }
  
  //  cout<<"Bound min:"<<boundMin_<<" BoundMax:"<<boundMax_<<endl;
  //     vector<float> xy_value[2];
  //     for (unsigned int i=0;i<2;i++) {
  //       for (unsigned int j=0;j<4;j++) {
  //         xy_value[i].push_back(corners[j][i]);
  //       }
  //       std::sort(xy_value[i].begin(), xy_value[i].end());
  //     }
  //     boundMin_[0]=xy_value[0][1]; boundMin_[1]=xy_value[1][1]; boundMin_[2] = 1;
  //     boundMax_[0]=xy_value[0][2]; boundMax_[1]=xy_value[1][2]; boundMax_[2] = 1;
  //   cout<<"Sorted Bound min:"<<boundMin_<<" BoundMax:"<<boundMax_<<endl;

  bool wrapAround = false;
  
  //detect wrap around in y here
  if(boundMax_[1] > boundMin_[1] && 
     boundMax_[1]-dTargetHeight_/2 > boundMin_[1]){
    cout<<"wrap around detected!"<<endl;
    
    //x stays the same
    boundMinWrap_[0] = boundMin_[0];
    boundMaxWrap_[0] = boundMax_[0];
    boundMin_[0] = boundMin_[0];
    boundMax_[0] = boundMax_[0]; 

    //y changes
    boundMinWrap_[1] = boundMax_[1];    
    boundMaxWrap_[1] = dTargetHeight_-2;

    boundMax_[1] = boundMin_[1];
    boundMin_[1] = 1;

    wrapAround=true;
  }

  
  //project depth and texture to cylindric panorama via unproject/project
  BIAS::HomgPoint2D point2d_src;
  BIAS::HomgPoint3D point3d;
  BIAS::HomgPoint2D point2d_dst;
  float depth_src=0.0;
  for (int y=uly_;y<lry_;y++) {
    for (int x=ulx_;x<lrx_;x++) {      
      point2d_src[0] = x;
      point2d_src[1] = y;
      point2d_src[2] = 1;
      depth_src = idaSource_[y][x];
      if (depth_src > 500) {
        point3d =(BIAS::HomgPoint3D)SourceP_.UnProjectToPoint(point2d_src, depth_src);
        if (SinkP_.DoesPointProjectIntoImage(point3d, point2d_dst)) {
          //cout<<"Point:"<<point3d<<" projects to sink at:"<<point2d_dst<<endl;
          point2d_dst.Homogenize();
          int xdst = point2d_dst[0];
          int ydst = point2d_dst[1];
          idaIntermediate_[ydst][xdst] = depth_src;
          if (withTex_) {
            for (unsigned int i=0;i<dTexChannels_;i++)
              idaIntermediateTex_[ydst][xdst*dTexChannels_+i] = idaTexIn_[y][x*dTexChannels_+i];
          }
        }
        // else
        //           cout<<"Point:"<<point3d<<" does not project to sink!"<<endl;
      }
    }
  }

  // fill black areas with neighbouring pixels in DEPTH image
  if (fillblack_) {
    for (unsigned int y=boundMin_[1]+fillAreaWindowSize_;
         y<boundMax_[1]-fillAreaWindowSize_;y++) {
      for (unsigned int x=boundMin_[0]+fillAreaWindowSize_;
           x<boundMax_[0]-fillAreaWindowSize_;x++) {
        if (idaIntermediate_[y][x] == 0) {
          for (unsigned int mod=1;mod<fillAreaWindowSize_;mod++) {
            if (idaIntermediate_[y][x+mod] != 0) {
              idaTarget_[y][x] = idaIntermediate_[y][x+mod];
              break;
            }
            if (idaIntermediate_[y][x-mod] != 0) {
              idaTarget_[y][x] = idaIntermediate_[y][x-mod]; 
              break;
            }
            if (idaIntermediate_[y+mod][x+mod] != 0) {
              idaTarget_[y][x] = idaIntermediate_[y+mod][x+mod]; 
              break;
            }
            if (idaIntermediate_[y+mod][x] != 0) {
              idaTarget_[y][x] = idaIntermediate_[y+mod][x];
              break;
            }
            if (idaIntermediate_[y+mod][x-mod] != 0) {
              idaTarget_[y][x] = idaIntermediate_[y+mod][x-mod]; 
              break;
            }
            if (idaIntermediate_[y-mod][x+mod] != 0) {
              idaTarget_[y][x] = idaIntermediate_[y-mod][x+mod]; 
              break;
            }
            if (idaIntermediate_[y-mod][x] != 0) {
              idaTarget_[y][x] = idaIntermediate_[y-mod][x]; 
              break;
            }
            if (idaIntermediate_[y-mod][x-mod] != 0) {
              idaTarget_[y][x] = idaIntermediate_[y-mod][x-mod]; 
              break;
            }
          }
        } else {
          idaTarget_[y][x] = idaIntermediate_[y][x];
        }
      }
    }
 

    if(wrapAround){
      for (unsigned int y=boundMinWrap_[1]+fillAreaWindowSize_;
           y<boundMaxWrap_[1]-fillAreaWindowSize_;y++) {
        for (unsigned int x=boundMinWrap_[0]+fillAreaWindowSize_;
             x<boundMaxWrap_[0]-fillAreaWindowSize_;x++) {
          if (idaIntermediate_[y][x] == 0) {
            for (unsigned int mod=1;mod<fillAreaWindowSize_;mod++) {
              if (idaIntermediate_[y][x+mod] != 0) {
                idaTarget_[y][x] = idaIntermediate_[y][x+mod];
                break;
              }
              if (idaIntermediate_[y][x-mod] != 0) {
                idaTarget_[y][x] = idaIntermediate_[y][x-mod]; 
                break;
              }
              if (idaIntermediate_[y+mod][x+mod] != 0) {
                idaTarget_[y][x] = idaIntermediate_[y+mod][x+mod]; 
                break;
              }
              if (idaIntermediate_[y+mod][x] != 0) {
                idaTarget_[y][x] = idaIntermediate_[y+mod][x];
                break;
              }
              if (idaIntermediate_[y+mod][x-mod] != 0) {
                idaTarget_[y][x] = idaIntermediate_[y+mod][x-mod]; 
                break;
              }
              if (idaIntermediate_[y-mod][x+mod] != 0) {
                idaTarget_[y][x] = idaIntermediate_[y-mod][x+mod]; 
                break;
              }
              if (idaIntermediate_[y-mod][x] != 0) {
                idaTarget_[y][x] = idaIntermediate_[y-mod][x]; 
                break;
              }
              if (idaIntermediate_[y-mod][x-mod] != 0) {
                idaTarget_[y][x] = idaIntermediate_[y-mod][x-mod]; 
                break;
              }
            }
          } else {
            idaTarget_[y][x] = idaIntermediate_[y][x];
          }
        }
      }
    }
  }
  // fill black areas with neighbouring pixels in TEXTURE image
  if (withTex_ && fillblack_) {
    if (dTexChannels_ == 1) {
      for (unsigned int y=boundMin_[1]+fillAreaWindowSize_;
           y<boundMax_[1]-fillAreaWindowSize_;y++) {
        for (unsigned int x=boundMin_[0]+fillAreaWindowSize_;
             x<boundMax_[0]-fillAreaWindowSize_;x++) {
          if (idaIntermediate_[y][x] == 0) {
            for (unsigned int mod=1;mod<fillAreaWindowSize_;mod++) {
              if (idaIntermediateTex_[y][x+mod] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y][x+mod];
                break;
              }
              if (idaIntermediateTex_[y][x-mod] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y][x-mod];
                break;
              }
              if (idaIntermediateTex_[y+mod][x+mod] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y+mod][x+mod];
                break;
              }
              if (idaIntermediateTex_[y+mod][x] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y+mod][x];
                break;
              }
              if (idaIntermediateTex_[y+mod][x-mod] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y+mod][x-mod];
                break;
              }
              if (idaIntermediateTex_[y-mod][x+mod] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y-mod][x+mod];
                break;
              }
              if (idaIntermediateTex_[y-mod][x] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y-mod][x];
                break;
              }
              if (idaIntermediateTex_[y-mod][x-mod] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y-mod][x-mod];
                break;
              }
            }
          } else {
            idaTexOut_[y][x] = idaIntermediateTex_[y][x];
          }
        }
      }
    } else {
      for (unsigned int y=boundMin_[1]+fillAreaWindowSize_;
           y<boundMax_[1]-fillAreaWindowSize_;y++) {
        for (unsigned int x=((int)boundMin_[0]+fillAreaWindowSize_)*dTexChannels_;
             x<((int)boundMax_[0]-fillAreaWindowSize_)*dTexChannels_;
             x+=dTexChannels_) {
          unsigned int val = 0;
          val = GetSummedValue_(&(idaIntermediateTex_[y][x]));
          if (val == 0) {
            for (unsigned int mod=1;mod<fillAreaWindowSize_;mod++) {
              unsigned int xmod = mod * dTexChannels_;
              if (GetSummedValue_(&(idaIntermediateTex_[y][x+xmod])) != 0) {
                SetValue_(&(idaIntermediateTex_[y][x+xmod]), &(idaTexOut_[y][x]));
                break;
              }
              if (GetSummedValue_(&(idaIntermediateTex_[y][x-xmod])) != 0) {
                SetValue_(&(idaIntermediateTex_[y][x-xmod]), &(idaTexOut_[y][x]));
                break;
              }
              if (GetSummedValue_(&(idaIntermediateTex_[y+mod][x+xmod])) != 0) {
                SetValue_(&(idaIntermediateTex_[y+mod][x+xmod]), &(idaTexOut_[y][x]));
                break;
              }
              if (GetSummedValue_(&(idaIntermediateTex_[y+mod][x])) != 0) {
                SetValue_(&(idaIntermediateTex_[y+mod][x]), &(idaTexOut_[y][x]));
                break;
              }
              if (GetSummedValue_(&(idaIntermediateTex_[y+mod][x-xmod])) != 0) {
                SetValue_(&(idaIntermediateTex_[y+mod][x-xmod]), &(idaTexOut_[y][x]));
                break;
              }
              if (GetSummedValue_(&(idaIntermediateTex_[y-mod][x+xmod])) != 0) {
                SetValue_(&(idaIntermediateTex_[y-mod][x+xmod]), &(idaTexOut_[y][x]));
                break;
              }
              if (GetSummedValue_(&(idaIntermediateTex_[y-mod][x])) != 0) {
                SetValue_(&(idaIntermediateTex_[y-mod][x]), &(idaTexOut_[y][x]));
                break;
              }
              if (GetSummedValue_(&(idaIntermediateTex_[y-mod][x-xmod])) != 0) {
                SetValue_(&(idaIntermediateTex_[y-mod][x-xmod]), &(idaTexOut_[y][x]));
                break;
              }
            }
          } else {
            SetValue_(&(idaIntermediateTex_[y][x]), &(idaTexOut_[y][x]));
          }
        }
      }
    }


  if(wrapAround){
  if (dTexChannels_ == 1) {
      for (unsigned int y=boundMinWrap_[1]+fillAreaWindowSize_;
           y<boundMaxWrap_[1]-fillAreaWindowSize_;y++) {
        for (unsigned int x=boundMinWrap_[0]+fillAreaWindowSize_;
             x<boundMaxWrap_[0]-fillAreaWindowSize_;x++) {
          if (idaIntermediate_[y][x] == 0) {
            for (unsigned int mod=1;mod<fillAreaWindowSize_;mod++) {
              if (idaIntermediateTex_[y][x+mod] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y][x+mod];
                break;
              }
              if (idaIntermediateTex_[y][x-mod] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y][x-mod];
                break;
              }
              if (idaIntermediateTex_[y+mod][x+mod] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y+mod][x+mod];
                break;
              }
              if (idaIntermediateTex_[y+mod][x] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y+mod][x];
                break;
              }
              if (idaIntermediateTex_[y+mod][x-mod] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y+mod][x-mod];
                break;
              }
              if (idaIntermediateTex_[y-mod][x+mod] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y-mod][x+mod];
                break;
              }
              if (idaIntermediateTex_[y-mod][x] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y-mod][x];
                break;
              }
              if (idaIntermediateTex_[y-mod][x-mod] != 0) {
                idaTexOut_[y][x] = idaIntermediateTex_[y-mod][x-mod];
                break;
              }
            }
          } else {
            idaTexOut_[y][x] = idaIntermediateTex_[y][x];
          }
        }
      }
    } else {
      for (unsigned int y=boundMinWrap_[1]+fillAreaWindowSize_;
           y<boundMaxWrap_[1]-fillAreaWindowSize_;y++) {
        for (unsigned int x=((int)boundMinWrap_[0]+fillAreaWindowSize_)*dTexChannels_;
             x<((int)boundMaxWrap_[0]-fillAreaWindowSize_)*dTexChannels_;
             x+=dTexChannels_) {
          unsigned int val = 0;
          val = GetSummedValue_(&(idaIntermediateTex_[y][x]));
          if (val == 0) {
            for (unsigned int mod=1;mod<fillAreaWindowSize_;mod++) {
              unsigned int xmod = mod * dTexChannels_;
              if (GetSummedValue_(&(idaIntermediateTex_[y][x+xmod])) != 0) {
                SetValue_(&(idaIntermediateTex_[y][x+xmod]), &(idaTexOut_[y][x]));
                break;
              }
              if (GetSummedValue_(&(idaIntermediateTex_[y][x-xmod])) != 0) {
                SetValue_(&(idaIntermediateTex_[y][x-xmod]), &(idaTexOut_[y][x]));
                break;
              }
              if (GetSummedValue_(&(idaIntermediateTex_[y+mod][x+xmod])) != 0) {
                SetValue_(&(idaIntermediateTex_[y+mod][x+xmod]), &(idaTexOut_[y][x]));
                break;
              }
              if (GetSummedValue_(&(idaIntermediateTex_[y+mod][x])) != 0) {
                SetValue_(&(idaIntermediateTex_[y+mod][x]), &(idaTexOut_[y][x]));
                break;
              }
              if (GetSummedValue_(&(idaIntermediateTex_[y+mod][x-xmod])) != 0) {
                SetValue_(&(idaIntermediateTex_[y+mod][x-xmod]), &(idaTexOut_[y][x]));
                break;
              }
              if (GetSummedValue_(&(idaIntermediateTex_[y-mod][x+xmod])) != 0) {
                SetValue_(&(idaIntermediateTex_[y-mod][x+xmod]), &(idaTexOut_[y][x]));
                break;
              }
              if (GetSummedValue_(&(idaIntermediateTex_[y-mod][x])) != 0) {
                SetValue_(&(idaIntermediateTex_[y-mod][x]), &(idaTexOut_[y][x]));
                break;
              }
              if (GetSummedValue_(&(idaIntermediateTex_[y-mod][x-xmod])) != 0) {
                SetValue_(&(idaIntermediateTex_[y-mod][x-xmod]), &(idaTexOut_[y][x]));
                break;
              }
            }
          } else {
            SetValue_(&(idaIntermediateTex_[y][x]), &(idaTexOut_[y][x]));
          }
        }
      }
    }
  }

  }

  return 0;
}

template <class InputStorageType, class OutputStorageType> 
unsigned int ForwardMappingNearestNeighbour<InputStorageType, OutputStorageType>::
GetSummedValue_(const OutputStorageType* pixel) {
  unsigned int ret=0;
  for (unsigned int i=0;i<dTexChannels_;i++) {
    ret+=pixel[i];
  }
  return ret;
}

template <class InputStorageType, class OutputStorageType> 
void ForwardMappingNearestNeighbour<InputStorageType, OutputStorageType>::
SetValue_(const OutputStorageType* in, OutputStorageType* out) {
  for (unsigned int i=0;i<dTexChannels_;i++) {
    out[i]=in[i];
  }
}

namespace BIAS{
template class ForwardMappingNearestNeighbour<unsigned char, unsigned char>;
template class ForwardMappingNearestNeighbour<float, float>;
template class ForwardMappingNearestNeighbour<unsigned char, float>;
template class ForwardMappingNearestNeighbour<float, unsigned char>;
}