SphericalDepthPanorama.cpp

#include "Image/SphericalDepthPanorama.hh"
#include <Base/Image/ImageConvert.hh>
#include <Base/Image/ImageIO.hh>
#include <Filter/Rescale.hh>
#include "Utils/ThreeDOut.hh"
#include <bias_config.h>

using namespace BIAS;
using namespace std;

/** Constructor */
SphericalDepthPanorama::SphericalDepthPanorama(Projection *initialProjection_){


    bFirstPoseSet_ = false; 
    pSphericalProj_ = NULL; 
    bTextureFilled_ = false;
    pInitialProj_ = initialProjection_;
    if (pInitialProj_ != NULL) {
        pSphericalProj_ = new BIAS::Projection(*pInitialProj_); 
        pSphericalProj_->GetParameters()->GetImageSize(panoramaWidth_,panoramaHeight_);
        panorama_.Init(panoramaWidth_,panoramaHeight_,1);
        panoramaHitCount_.Init(panoramaWidth_,panoramaHeight_,1);
        panoramaHitCount_.FillImageWithConstValue(0.0);
        panoramaTexture_.Init(panoramaWidth_,panoramaHeight_,1);

        //contruct spherical projection      
        pSphericalProj_->GetParameters()->SetQC(pInitialProj_->GetQ(),
            pInitialProj_->GetC());
        bFirstPoseSet_ = true;
    }


}

/** Destructor */
SphericalDepthPanorama::~SphericalDepthPanorama(){
 
  if(pSphericalProj_){
    delete pSphericalProj_;
    pSphericalProj_ = NULL;
  }
}


int  SphericalDepthPanorama::
ProcessSphericalProjection(BIAS::Image<float> &depthMap,
                           BIAS::Projection & p,
                           BIAS::Image<unsigned char> *texture){
  if(!bFirstPoseSet_){
    pSphericalProj_ = new BIAS::Projection(p);    
    pSphericalProj_->GetParameters()->GetImageSize(panoramaWidth_,panoramaHeight_);
    panorama_.Init(panoramaWidth_,panoramaHeight_,1);
    panoramaHitCount_.Init(panoramaWidth_,panoramaHeight_,1);
    panoramaHitCount_.FillImageWithConstValue(0.0);
    panoramaTexture_.Init(panoramaWidth_,panoramaHeight_,1);
    
    //contruct spherical projection      
    pSphericalProj_->GetParameters()->SetQC(p.GetQ(),
                                            p.GetC());
    bFirstPoseSet_ = true;
  }
 // cout<<"Initial projection: "<<*pSphericalProj_<<endl;
 // cout<<"Current projection: "<<p<<endl;
  int ret =0;
  float **ida = depthMap.GetImageDataArray();
  float **idaP = panorama_.GetImageDataArray();
  float **idaPHC = panoramaHitCount_.GetImageDataArray();
  unsigned char ** idaTex = panoramaTexture_.GetImageDataArray();

  int width = depthMap.GetWidth();
  int height = depthMap.GetHeight();
 
  float depthValue=0.0;
  BIAS::HomgPoint3D point3D;  
  BIAS::HomgPoint2D point2D,sphericalPoint;  

  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){
          idaTex[y][x] = texture->GetImageDataArray()[y][x];         
          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;
        }
      }
    }
  }
  
/*  
  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) {     
      if (depthValue > 0.0) {     
        // Unproject to a 3Dpoint
        point3D = p.UnProjectToPoint(point2D,depthValue);            
        //cout<<"Point :"<<x<<","<<y<<" Maps to 3D Point"<<point3D<<endl;
        if(pSphericalProj_->DoesPointProjectIntoImage(point3D,sphericalPoint)){ 
          sphericalPoint.Homogenize();
          x1 = (unsigned int)sphericalPoint[0];
          y1 = (unsigned int)sphericalPoint[1];       
      //cout<<"3DPoint maps to 2D spherical point:"<<x1<<","<<y1<<endl;            
          if(x1>=0.0 && x1<panoramaWidth_ && 
             y1>=0.0 && y1<panoramaHeight_ ) {

            //fill texture panorama with values
            if(texture != NULL){
              idaTex[y1][x1] = texture->GetImageDataArray()[y][x];         
              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;
            }
          }
        }
        //else
        //  cout<<"point3D:"<<point3D<<" has no 2D point!"<<endl;
      }
    }
  }
  
  
  */
  return ret;
}


int  SphericalDepthPanorama::
GetPanorama(BIAS::Image<float> &image, 
                BIAS::Image<unsigned char> &texture){
  int ret =0;
  image = panorama_; 
  image.ScaleShiftBetween(0,255);
  //BIAS::ImageIO::Save("pan.mip",image);
  if(bTextureFilled_){
    texture = panoramaTexture_;
    //  BIAS::ImageIO::Save("tex.mip",panoramaTexture_);
  }
  else{
    BIAS::ImageConvert::ConvertST(panorama_,texture,
                                  BIAS::ImageBase::ST_unsignedchar);
    texture.ScaleShiftBetween(0,255);
  }
  return ret;
}


int SphericalDepthPanorama::
GetTriangleMesh(BIAS::TriangleMesh &mesh){
  int ret =-1; 
  BIAS::Image<unsigned char> tmpUC;
  if(bTextureFilled_){
    if(pSphericalProj_ != NULL){
      int factor = 4;
      BIAS::Projection proj(*pSphericalProj_);
      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_;

      BIAS::ImageIO::Save("Panorama_DepthMap.mip", panorama_);
      BIAS::ImageIO::Save("Panorama_DepthMap_Downsampled.mip", depthDownSampled_);
      BIAS::ImageIO::Save("Panorama_Texture.mip", panoramaTexture_);
#ifdef BIAS_HAVE_XML2
      proj.XMLWrite("Panorama_Projection.xml");
#else
    BIASERR("Could not save because of missing XML support in BIAS.");
#endif
      proj.Rescale((float)factor);
      mesh.GenerateDenseMesh(depthDownSampled_,proj,panoramaTexture_,0,0, depthDownSampled_.GetWidth() ,depthDownSampled_.GetHeight());
      proj.Rescale(1.0f/float(factor));
      //      mesh.GenerateDenseMesh(panorama_,proj,panoramaTexture_,0,0,
      //                             panoramaWidth_,panoramaHeight_);
      //      mesh.GenerateSimplifiedMesh(panorama_,
      //                                   proj,panoramaTexture_,0,0,
      //                                   panoramaWidth_,panoramaHeight_);

      return 0;
    }  
    else
      return ret;

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

      return 0;
    }
    else
      return ret;
    
  } 
}