CylindricDepthPanorama.cpp

#include <Image/CylindricDepthPanorama.hh>
#include <Base/Image/ImageConvert.hh>
#include <Base/Image/ImageIO.hh>
#include <Utils/ThreeDOut.hh>
#include <Image/ForwardMappingNearestNeighbour.hh>
#include <Base/Common/FileHandling.hh>

using namespace BIAS;
using namespace std;

static int imageCount=0;

CylindricDepthPanorama::
CylindricDepthPanorama(){
  inited_ = false;
  fillArea_ = 10;
  bCostImageCreated_=false;
}

/** Constructor */
CylindricDepthPanorama::
CylindricDepthPanorama(const BIAS::Projection& initialProjection, 
                       unsigned int channels)
{
  inited_ = false;
  fillArea_ = 10;
  bCostImageCreated_=false;
  Init(initialProjection,channels);  
}

CylindricDepthPanorama::
CylindricDepthPanorama(const BIAS::ProjectionParametersCylindric& iP,
                       unsigned int channels)
{
  inited_ = false;
  fillArea_ = 10;
  bCostImageCreated_=false;
  Init(iP,channels);  
}

int CylindricDepthPanorama::
Init(const ProjectionParametersCylindric& iP, 
     unsigned int channels)
{
  Projection p(iP);
  return Init(p,channels);
}

int CylindricDepthPanorama::
Init(const Projection& iP, unsigned int channels){
  if((dynamic_cast<const ProjectionParametersCylindric*>(iP.GetParameters()))== NULL){
    BIASERR("Not a cylindric projection.");
    return -1;
  }

  bTextureFilled_ = false;
  bFirstPoseSet_ = false;
  dTexChannels_ = channels;
  initialProjection_ = iP;
  pCylinderProj_ = (ProjectionParametersCylindric*)(initialProjection_.GetParameters());
  pCylinderProj_->GetImageSize(panoramaWidth_,panoramaHeight_);

  panorama_.Init(panoramaWidth_,panoramaHeight_,1);
  panoramaHitCount_.Init(panoramaWidth_,panoramaHeight_,1);
  panoramaHitCount_.FillImageWithConstValue(0.0);
  panoramaTexture_.Init(panoramaWidth_,panoramaHeight_,dTexChannels_);

  initialProjection_.GetParameters()->SetPose(iP.GetParameters()->GetPose());    
  pCylinderProj_->SetPose(iP.GetParameters()->GetPose());
  bFirstPoseSet_ = true;  
  inited_= true;
 
  return 0;
}

/** Destructor */
CylindricDepthPanorama::~CylindricDepthPanorama(){  
}

void CylindricDepthPanorama::
SetFillArea(int area){
  fillArea_ = area;
}

int  CylindricDepthPanorama::
ProcessCylindricProjection(BIAS::Image<float> &depthMap,
                           BIAS::Projection & p,
                           BIAS::Image<unsigned char> *texture,
                           bool useProjection){

  int ret =0;
  if(!bFirstPoseSet_){
    //contruct cylindrical projection      
    initialProjection_ = BIAS::Projection(p);    
    pCylinderProj_->SetQC(initialProjection_.GetQ(),initialProjection_.GetC());
    bFirstPoseSet_ = true;
  }

  if(!bCostImageCreated_){     
    float w=(float)depthMap.GetWidth();
    float h=(float)depthMap.GetHeight();
    float wh =(float)(w/2.0);
    float hh =(float)(h/2.0);

    costIm_.Init((unsigned)rint(w),(unsigned)rint(h),3);
    float ** ida = costIm_.GetImageDataArray();
    double radius=0.0;
    double maxradius = sqrt((wh)*(wh) + (hh)*(hh));
  
    for(unsigned x=0;x<(unsigned)w;x++){
      for(unsigned y=0;y<(unsigned)h;y++){
        radius=sqrt((x-wh)*(x-wh) + (y-hh)*(y-hh));

        if(radius <=0) radius=1e-10; 
        ida[y][x*3]= float((maxradius-radius)/maxradius)*255.0f;
        ida[y][x*3+1]= float((maxradius-radius)/maxradius)*255.0f;
        ida[y][x*3+2]= float((maxradius-radius)/maxradius)*255.0f;
      }
    }
    bCostImageCreated_=true;
  }

  BIAS::ForwardMappingNearestNeighbour<float,float> fm;
  Projection pDest(*pCylinderProj_);
  BIAS::Image<float> textureImage;
  BIAS::Image<float> tmpTex,tmpCost;
  stringstream file,file1,file2;//file3;
  //forward map images to cylinder
  fm.SetSourceCam(p);
  fm.SetSinkCam(pDest,3);
  fm.SetFillAreaWindowSize(fillArea_);
  BIAS::ImageConvert::ConvertST(*texture,textureImage,ImageBase::ST_float);
  fm.Map(&depthMap,destDepth_,&textureImage,tmpTex);
  BIAS::ImageConvert::ConvertST(tmpTex,destTex_,ImageBase::ST_unsignedchar);
 
  fm.Map(&depthMap,destDepth_,&costIm_,tmpCost);

  file<<"TexturePanorama-"<<FileHandling::LeadingZeroString(imageCount,5)
      <<".mip";
  file1<<"DepthPanorama-"<<FileHandling::LeadingZeroString(imageCount,5)
       <<".mip";
  file2<<"CostPanorama-"<<FileHandling::LeadingZeroString(imageCount,5)
       <<".mip";
  //  file3<<"Cost-"<<FileHandling::LeadingZeroString(imageCount,5)
  //        <<".mip";
  //BIAS::ImageIO::Save(file.str(),destTex_);
  //BIAS::ImageIO::Save(file1.str(),destDepth_);
  //BIAS::ImageIO::Save(file2.str(),tmpCost);
  BIAS::ImageIO::Save(file.str(),destTex_);
  BIAS::ImageIO::Save(file1.str(),destDepth_);
  BIAS::ImageIO::Save(file2.str(),tmpCost);
  // BIAS::ImageIO::Save(file3.str(),costIm_);
  
  float **ida = destDepth_.GetImageDataArray();
  unsigned char **idaDestTex = destTex_.GetImageDataArray(); 
  float **idaP = panorama_.GetImageDataArray();
  float **idaPHC = panoramaHitCount_.GetImageDataArray();
  unsigned char** idaTex = panoramaTexture_.GetImageDataArray();

  int width = destDepth_.GetWidth();
  int height = destDepth_.GetHeight();
 
  float depthValue=0.0;
  BIAS::HomgPoint3D point3D;  
  BIAS::HomgPoint2D point2D,cylPoint;
  
  unsigned channels = destTex_.GetChannelCount();

  if (!useProjection) {  
    for(int x=0;x<width;x++){
      for(int y=0;y<height;y++){      
        depthValue = ida[y][x];   
        if (depthValue > 0.0) {     
          //fill texture panorama with values
          if(texture != NULL){
            for (unsigned int i=0;i<channels;i++) {
              unsigned char val = idaDestTex[y][x*channels+i];
              if (val != 0)
                idaTex[y][x*channels+i] = val;
              
            }
            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;
          }
        }
      }
    }
  } else {
    unsigned int x1,y1;
    for(int x=0;x<width;x++){
      for(int y=0;y<height;y++){      
        point2D[0]=x;
        point2D[1]=y;
        point2D[2]=1;            
        depthValue = ida[y][x];   
        if (depthValue != 0.0) {       
          // Unproject to a 3Dpoint
          point3D = p.UnProjectToPoint(point2D,depthValue);            
          cylPoint = pCylinderProj_->Project(point3D);
          cylPoint.Homogenize();
          x1 = (unsigned int)cylPoint[0];
          y1 = (unsigned int)cylPoint[1];       
            
          //   cout<<"3DPoint: "<<point3D<<" Cylindercoords: "<<cylPoint<<endl;
                           
          if(x1>=0.0 && x1<panoramaWidth_ && 
             y1>=0.0 && y1<panoramaHeight_ ) {
            //fill texture panorama with values
            if(texture != NULL){
              for (unsigned int c=0;c<dTexChannels_;c++)
                idaTex[y1][x1*dTexChannels_+c] = texture->GetImageDataArray()[y][x*dTexChannels_+c];         
              bTextureFilled_ = true;
            }
            if(idaPHC[y1][x1] == 0.0){
              idaP[y1][x1]=depthValue;
              idaPHC[y1][x1]=1.0;
            }
            else{          
              idaP[y1][x1]= 
                (idaP[y1][x1] * idaPHC[y1][x1]+depthValue)/(idaPHC[y1][x1]+1);
              idaPHC[y1][x1]+=1.0;
            }
          }
        }
      }
    }
  }
  
  imageCount++;
  return ret;
}

int CylindricDepthPanorama::
GetMappedImages(BIAS::Image<float> &image, 
                BIAS::Image<unsigned char> &texture){
  if(!inited_) return -1;
  image = destDepth_;
  texture = destTex_;
  return 0;
}

int CylindricDepthPanorama::
GetPanorama(BIAS::Image<float> &image, 
            BIAS::Image<unsigned char> &texture){
  if(!inited_) return -1;
  
  image = panorama_;
  if(bTextureFilled_){
    texture = panoramaTexture_;   
  }
  else{
    BIAS::ImageConvert::ConvertST(panorama_,texture,
                                  BIAS::ImageBase::ST_unsignedchar);
    texture.ScaleShiftBetween(0,255);
  }
  return 0;
}


int CylindricDepthPanorama::
GetTriangleMesh(BIAS::TriangleMesh &mesh){
  if(!inited_) return -1;
  int ret =-1; 
  BIAS::Image<unsigned char> tmpUC;
  Projection outProj (initialProjection_);
  if(bTextureFilled_){
  
    int factor = 4;
    outProj.Rescale((float)factor);
    BIAS::Image<float> depthDownSampled_(panoramaWidth_ / factor, panoramaHeight_ / factor);
    BIAS::Image<unsigned char> textureDownSampled_(panoramaWidth_ / factor, panoramaHeight_ / factor);
    Rescale<float, float> pRescaleFilter_;
    Rescale<unsigned char,unsigned char> pRescaleFilterUC_;
    pRescaleFilter_.SetFactor((float)factor);
    pRescaleFilter_.SetLowPassType(LPT_Gauss);
    pRescaleFilter_.Filter(panorama_, depthDownSampled_);
    pRescaleFilterUC_.SetFactor((float)factor);
    pRescaleFilterUC_.SetLowPassType(LPT_Gauss);
    pRescaleFilterUC_.Filter(panoramaTexture_, textureDownSampled_);

    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_;

#ifdef BIAS_DEBUG
    BIAS::ImageIO::Save("Panorama_DepthMap.mip", panorama_);
    BIAS::ImageIO::Save("Panorama_DepthMap_Downsampled.mip", depthDownSampled_);
    BIAS::ImageIO::Save("Panorama_Texture.mip", panoramaTexture_);
#endif
    mesh.GenerateDenseMesh(depthDownSampled_, outProj,textureDownSampled_,
                           0,0, depthDownSampled_.GetWidth() ,depthDownSampled_.GetHeight());   
    //   mesh.GenerateDenseMesh(panorama_,outProj,panoramaTexture_,0,0,
    //                              panoramaWidth_,panoramaHeight_);

    //   mesh.GenerateSimplifiedMesh(panorama_,
    //                               outProj,panoramaTexture_,0,0,
    //                               panoramaWidth_,panoramaHeight_);

    return 0;
   
  }
  else{
    BIAS::ImageConvert::ConvertST(panorama_,tmpUC,
                                  BIAS::ImageBase::ST_unsignedchar);
    tmpUC.ScaleShiftBetween(0,255);
    if(pCylinderProj_ != NULL){
      BIAS::Projection proj(*pCylinderProj_);
      mesh.GenerateDenseMesh(panorama_,proj,tmpUC,0,0,
                             panoramaWidth_,panoramaHeight_);
      //       mesh.GenerateSimplifiedMesh(panorama_,
      //                                   proj,panoramaTexture_,0,0,
      //                                   panoramaWidth_,panoramaHeight_);

      return 0;
    }
    else
      return ret;
    
  } 
}