ImageWarper.cpp

/*
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003, 2004    (see file CONTACTS for details)
Multimediale Systeme der Informationsverarbeitung
Institut fuer Informatik
Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "ImageWarper.hh"
#include <OpenGLFramework/Base/glfBatch.hh>
#include <Base/Image/ImageIO.hh>
#include <Base/Debug/TimeMeasure.hh>

using namespace BIAS;
using namespace std;


template <class StorageType>
ImageWarper<StorageType>::
ImageWarper(bool ignoreRadialDistortion){
    ignoreRadialDist_ = ignoreRadialDistortion;
  inited_=false;
}

template <class StorageType>
ImageWarper<StorageType>::
~ImageWarper(){
  pBuffer_.Destroy();
}


template <class StorageType> int
ImageWarper<StorageType>::
Init(BIAS::ProjectionParametersPerspective& source,
     BIAS::ProjectionParametersPerspective& target,
     float minZ, float maxZ){
  //init context
  glfRenderingContextConfig cfg;
  //TODO: Did this do something in the past ???
        /*int depthbits;
    GLenum depthTextureFormat = 0;
    glGetIntegerv(GL_DEPTH_BITS, &depthbits);
    switch (depthbits) {
    case 16:
        depthTextureFormat = GL_DEPTH_COMPONENT16;
        break;
    case 24:
        depthTextureFormat = GL_DEPTH_COMPONENT24;
        break;
    case 32:
        depthTextureFormat = GL_DEPTH_COMPONENT32;
        break;
        }*/
  cfg.width         = 5;
  cfg.height        = 5;
  cfg.doubleBuffer  = 0;
  cfg.redSize       = 8;
  cfg.greenSize     = 8;
  cfg.blueSize      = 8;
  cfg.alphaSize     = 8;
  cfg.depthSize     = 24;
  cfg.stencilSize   = 0;
    //cout<<"Before pbuffer Init:"<<endl;
  // initialize context
    try {               
    pBuffer_.Init(cfg);
  }catch (glfException& e) {
    std::cout << "ImageWarper Init Error: " << e.GetMessageString() << std::endl;
    return -1;
  }
  
  target.GetImageSize(width_, height_);
  //cout<<"ImageWarper width, height:"<<width_<<", "<<height_<<endl;

    try {                  
    resultTexture.Create();
    resultTexture.SetMagFilter(GL_NEAREST);
    resultTexture.SetMinFilter(GL_NEAREST);
    resultTexture.SetWrapS(GL_CLAMP);
    resultTexture.SetWrapT(GL_CLAMP);
    resultTexture.Allocate(width_, height_, GL_RGB);
  }catch (glfException& e) {
    std::cout << "ImageWarper resultTexture error: " << e.GetMessageString() << std::endl;
    return -2;
  }
  try { 
    depthTexture.Create();
    depthTexture.SetMagFilter(GL_NEAREST);
    depthTexture.SetMinFilter(GL_NEAREST);
    depthTexture.SetWrapS(GL_CLAMP);
    depthTexture.SetWrapT(GL_CLAMP);
    depthTexture.Allocate(width_, height_, GL_DEPTH_COMPONENT);
  }catch (glfException& e) {
    std::cout << "ImageWarper depthTexture error: " << e.GetMessageString() << std::endl;
    return -3;
  }
  try { 
    depthWarpFBO_.Create();
    depthWarpFBO_.AttachTexture(resultTexture,GL_COLOR_ATTACHMENT0_EXT);
    depthWarpFBO_.AttachTexture(depthTexture,GL_DEPTH_ATTACHMENT_EXT);
    depthWarpFBO_.CheckComplete();
  }catch (glfException& e) {
    std::cout << "ImageWarper depthWarpFBO error: " << e.GetMessageString() << std::endl;
    return -4;
  }
  //init projections
  source_ = source;
  target_ = target;
  
  try { 
    if(ignoreRadialDist_)
      depthMeshing_.Init(source_,false);
    else{
      bool hasDist = true; //determine here
      depthMeshing_.Init(source_,hasDist);
    }
  }catch (glfException& e) {
    std::cout << "ImageWarper depthMeshing_.Init() error: " << e.GetMessageString() << std::endl;
    return -5;
  }
  try { 
    depthMeshing_.SetRenderTarget(&depthWarpFBO_);
    depthMeshing_.SetTargetCameraParameters(target_, minZ, maxZ);
    // depthMeshing_.SetTargetCameraParametersToGlobal(true);
  }catch (glfException& e) {
    std::cout << "ImageWarper depthMeshing_.SetRenderTarget error: " << e.GetMessageString() << std::endl;
    return -6;
  }
  inited_ = true;
 // cout<<"ImageWarper Init done."<<endl;
  return 0;
}


template <class StorageType> int
ImageWarper<StorageType>::
SetTargetCamera(BIAS::ProjectionParametersPerspective& target,
                float minZ, float maxZ){
  target_ = target;
  target_.GetImageSize(width_, height_);
  depthMeshing_.SetTargetCameraParameters(target_, minZ, maxZ);
  //depthMeshing_.SetTargetCameraParametersToGlobal(true);
  return 0;
}


template <class StorageType> int
ImageWarper<StorageType>::
ProcessWithColor(BIAS::Image<StorageType>& depthImage,
        BIAS::Image<unsigned char>& colorImage,
        BIAS::Image<StorageType>& warpedDepthImage,
        BIAS::Image<unsigned char>& warpedColorImage,
        float minZ, float maxZ, bool getdepth){
    
    int ret =0; 
    //do not change line order here!!
    glfBatch::SetDefaultRenderStates();
    pBuffer_.MakeCurrent();
    depthMeshing_.UploadProjectiveTexture(colorImage);  
    ret = Process(depthImage,warpedDepthImage,minZ,maxZ,getdepth);
    resultTexture.CopyToImage(warpedColorImage,0);
    return ret;
} 

template <class StorageType> int
ImageWarper<StorageType>::
Process(BIAS::Image<StorageType>& depthImage,
        BIAS::Image<StorageType>& warpedDepthImage,
        float minZ, float maxZ, bool getdepth){
  //    TimeMeasure timer;
  //    timer.Start();
  if(!inited_) return -1;
    //do not change line order here!!
  glfBatch::SetDefaultRenderStates();
  pBuffer_.MakeCurrent();
  glViewport(0,0, width_, height_);
  depthWarpFBO_.ClearColorBuffer();
  depthWarpFBO_.ClearDepthBuffer();
  depthMeshing_.UploadDepthMap(depthImage);
  
  //  timer.Stop();
  //  cout<<"ImageWarper::UploadDepthMap:"; timer.PrintRealTime();
  //  timer.Reset();
  //  timer.Start();
  //  
  depthMeshing_.Draw();
  
  //  timer.Stop();
  //  cout<<"ImageWarper::Draw:"; timer.PrintRealTime();
  //  timer.Reset();
  //  timer.Start();
  //    
  depthTexture.CopyToImage(warpedDepthImage,0);

  //  timer.Stop();
  //  cout<<"ImageWarper::CopyToImage:"; timer.PrintRealTime();
  //  timer.Reset();
  //  timer.Start();

    
  //translate to depth
  if(getdepth){
    float** z = warpedDepthImage.GetImageDataArray();
    Vector3<double> ray, pos;
    for (unsigned int y=0; y<warpedDepthImage.GetHeight(); y++) {
        for (unsigned int x=0; x<warpedDepthImage.GetWidth(); x++) {
            if (z[y][x] >= 1.0) z[y][x] = 0.0;
            else if(z[y][x] != 0.0){
                z[y][x] = minZ*maxZ / (maxZ - (z[y][x])*(maxZ-minZ));
                target_.UnProjectToRay(HomgPoint2D(x,y,1.0), pos, ray);
                ray = (z[y][x] / ray[2])*ray;
                z[y][x] = ray.NormL2();
            }
        }
    }
    //      timer.Stop();
    //      cout<<"ImageWarper::Translate:"; timer.PrintRealTime();
    //      timer.Reset();
    //      timer.Start();
  }
 
  
  return 0;
}


///////////////////////////////////////////////////

template <class StorageType> void
ImageWarper<StorageType>::
SetDefaultTextureParameters_(BIAS::glfTexture2D& tex) {
  //tex.SetMinFilter(GL_LINEAR_MIPMAP_LINEAR);//GL_NEAREST
  tex.SetMinFilter(GL_LINEAR);
  tex.SetMagFilter(GL_LINEAR);//GL_NEAREST
  tex.SetWrapS(GL_CLAMP);
  tex.SetWrapT(GL_CLAMP);
}

template <class StorageType> void
ImageWarper<StorageType>::
InitFBOTexture_(BIAS::glfTexture2D& tex,
                unsigned width, unsigned height,
                GLenum intFormat) {
  tex.Create();
  SetDefaultTextureParameters_(tex);
  tex.Allocate(width, height, intFormat);
}
///////////////////////////////////////////////////
template BIASOpenGLFramework_EXPORT class BIAS::ImageWarper<float>;