biasdepthwarp.cpp

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

 Copyright (C) 2003-2009    (see file CONTACT 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 <bias_config.h>
#include <Base/Image/Image.hh>
#include <Base/Image/ImageConvert.hh>
#include <Image/Camera.hh>
#include <Image/PMDImageProc.hh>
#include <Utils/Param.hh>
#include <Utils/IOUtils.hh>
#include <Base/Image/ImageIO.hh>
#include <Filter/Median.hh>
#include <Filter/Bilateral.hh>
#ifdef BIAS_HAVE_OPENGL
#include <OpenGLFramework/RenderingContext/glfPBuffer.hh>
#include <OpenGLFramework/SpecializedBatches/PMDWarp.hh>
#include <OpenGLFramework/Base/glfException.hh>
#include <OpenGLFramework/Base/glfFramebufferObject.hh>
#include <Gui/biasgl.h>
#endif

using namespace std;
using namespace BIAS;

/**
   @file
   @relates PMDWarp,glfFramebufferObject,glfPBuffer
   @brief Example for using the PMDWarp class. This example creates a glfPBuffer
   and warps a depth map into the perspective of another camera using meshing on the GPU
   see biasdepthwarp.cpp
   @ingroup g_tools
   @ingroup g_openglframework
   @author Bogomil Bartzcak
   @date 2009
*/
int main(int argc, char* argv[]) 
{
  Param params(true);

  string* tofImageName = 
    params.AddParamString("tofImage", "image of tof camera", "", 'i');
  string* ampImageName = 
     params.AddParamString("ampImage", "amplitude image of tof camera [optional]", "", 'a');
  
  string* targetImageName = 
    params.AddParamString("targetImage", "target image name", "result.mip", 'o');

  string* sourceProjName = 
    params.AddParamString("sourceProj", "projection with params of source tof cam, ATTENTION Distortion parameters are NOT IGNORED", "", 's');
  string* targetProjName = 
    params.AddParamString("targetProj", "projection with params of target perspective cam, ATTENTION Distortion parameters are IGNORED", "", 't');
  bool* translateToDepth = 
    params.AddParamBool("depth", "output depth instead of z image", false, 'd');
  bool* bUndist = 
    params.AddParamBool("undistort", "undistort depth", false, 'u');
  bool* bBilateral =
     params.AddParamBool("filterBilateral", "filter image with bilateral filter", false, 'b');
  bool* bMedian =
      params.AddParamBool("filterMedian", "filter image with median filter", false, 'm');
  bool* bSimple =
        params.AddParamBool("simpleProjectAndFill", "use simple point projection and dilation instead of GPU warping", false, 'S');

  int *kernelsize = params.AddParamInt("kernelsize","optional kernelsize for dilation in simple projection case",10,1,1000,'k');
  double* zNear = params.AddParamDouble("zNear","",0.1);
  double* zFar = params.AddParamDouble("zFar","",10000);

  if(!IOUtils::ParseCommandLineEvalHelp(params, argc, argv) ) 
    return 0;

  //members
  Image<float> tofImage,targetImage,ampImage;
  Image<unsigned char> texImage;
    Image<float> resultImg;

  Projection tofProj, targetProj;
  ProjectionParametersPerspectiveDepth* tofParam = NULL;
  ProjectionParametersPerspective* tofParamPersp = NULL;
  ProjectionParametersPerspective* targetParam = NULL;
  
  bool tofIsPersp = false;
  bool hasTexture = false;
  //load tof depth image
  if(BIAS::ImageIO::Load(*tofImageName,tofImage)!=0) {
    BIASERR("Could not load tof image:"<<*tofImageName);
    return -1;
  }

  // scale depth image
  //  if(tofImage.GetChannelCount() != 1)
  //    ImageConvert::IP_ToGrey(tofImage);
  //
  //  tofImage *=1000;

  //load tof amp image
  if(*ampImageName != ""){
    if(BIAS::ImageIO::Load(*ampImageName,ampImage)!=0) {
        BIASERR("Could not load tof image:"<<*ampImageName);
        return -1;
    }
    else{
        BIAS::ImageConvert::ConvertST(ampImage,texImage,ImageBase::ST_unsignedchar);
        hasTexture=true;
    }
  } 
  
  //load projections
  if(tofProj.Load(*sourceProjName)!=0) {
    BIASERR("Could not load source tof projection: "<<*sourceProjName);
    return -1;
  }
  if(targetProj.Load(*targetProjName)!=0) {
    BIASERR("Could not load target tof projection:"<<*targetProjName);
    return -1;
  }

  
  //source tof parameters
  if(!IOUtils::CheckType<ProjectionParametersPerspectiveDepth,
     ProjectionParametersBase>(tofProj.GetParameters(), tofParam)) {
      if(!IOUtils::CheckType<ProjectionParametersPerspective, 
         ProjectionParametersBase>(tofProj.GetParameters(), tofParamPersp)) {
        BIASERR("wrong type dynamic cast failed!");
        return -1;
      } else {
        tofIsPersp = true;
      }
  }
    //target parameters
  if(!IOUtils::CheckType<ProjectionParametersPerspective, 
     ProjectionParametersBase>(targetProj.GetParameters(), targetParam)) {
    BIASERR("wrong type dynamic cast failed!");
    return -1;
  }

//  cout<<"Deleting high variance values\n";
//  PMDImageProc::DeleteHighVarianceValues(tofImage,400,1);

  //fill holes from range ambiguity wrap
  cout<<"Setting all values below 500 to 10 000\n";
  tofImage.BelowThresholdToValue(500,10000);

  //undistort depth image
  if (*bUndist){
    cout<<"Undistorting depth image\n";
    tofParam->UnDistortDepthMapIP(tofImage);
  }

  //bilateral or mean filter image
  if (*bBilateral) {
    cout<<"Bilateral filtering image\n";
    Image<float> filterTmp;
    // take bilateral filter...
    Bilateral<float, float> bilateral;
    // set parameters
    bilateral.SetSigma(0.7);
    bilateral.SetBilateralSigma(150.0);
    bilateral.SetSize(3, 3);
    bilateral.SetIgnoreValue(0.0);

    for (unsigned int i = 0; i < 5; i++) {
         if(*ampImageName != "")
             bilateral.Filter(tofImage, ampImage, filterTmp);
         else
             bilateral.Filter(tofImage, filterTmp);
    }
  }

  if(*bMedian) {
    cout<<"Median filtering image\n";
    Image<float> filterTmp;
    Median<float, float> median;
    median.SetSize(3, 3);
    median.Filter(tofImage, filterTmp);
    tofImage = filterTmp;
  }


  //delete if out of range zNear , zFar
  float **ida = tofImage.GetImageDataArray();
  for (unsigned int y=0; y<tofImage.GetHeight(); y++) {
    for (unsigned int x=0; x<tofImage.GetWidth(); x++) {
      if (ida[y][x] < *zNear || ida[y][x] > *zFar) {
        ida[y][x] = 0;
      }
    }
  }
  


  //either use simple point projection and hole filling with dilation (bSimple)
  //or use PMDWarp with warping on GPU (else case)
  if(*bSimple){
    PMDImageProc::FitDepthTo2DImage(tofImage,targetProj,tofProj,*kernelsize);
    resultImg   = tofImage;
  }
  else
  {
#ifdef BIAS_HAVE_OPENGL
    //calculate relative params
    Pose relativePose;
    if(!tofIsPersp) {
        relativePose.BecomeRelativeTransform(targetParam->GetPose(), tofParam->GetPose());
    } else {
        relativePose.BecomeRelativeTransform(targetParam->GetPose(), tofParamPersp->GetPose());
    }
    targetParam->SetPose(relativePose);

    unsigned int width, height;
    targetParam->GetImageSize(width, height);

    glfPBuffer pbuffer;
    PMDWarp gpuDepthWarp;
    try {

        glfRenderingContextConfig cfg;
        cfg.width         = 5;
        cfg.height        = 5;
        cfg.doubleBuffer  = 0;
        cfg.redSize       = 8;
        cfg.greenSize     = 8;
        cfg.blueSize      = 8;
        cfg.alphaSize     = 0;
        cfg.depthSize     = 24;
        cfg.stencilSize   = 0;

        pbuffer.Init(cfg);

        glfTexture2D resultTexture;
        resultTexture.Create();
        resultTexture.SetMagFilter(GL_NEAREST);
        resultTexture.SetMinFilter(GL_NEAREST);
        resultTexture.SetWrapS(GL_CLAMP);
        resultTexture.SetWrapT(GL_CLAMP);
        resultTexture.Allocate(width, height, GL_RGB);

        glfTexture2D depthTexture;
        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);

        glfTexture2D dispTexture;
        dispTexture.Create();
        dispTexture.SetMagFilter(GL_NEAREST);
        dispTexture.SetMinFilter(GL_NEAREST);
        dispTexture.SetWrapS(GL_CLAMP);
        dispTexture.SetWrapT(GL_CLAMP);
        dispTexture.Allocate(width, height, GL_LUMINANCE32F_ARB);

        glfFramebufferObject fbo;
        fbo.Create();
        fbo.AttachTexture(resultTexture, GL_COLOR_ATTACHMENT0_EXT);
        fbo.AttachTexture(dispTexture, GL_COLOR_ATTACHMENT1_EXT);
        fbo.AttachTexture(depthTexture, GL_DEPTH_ATTACHMENT_EXT);
        fbo.CheckComplete();
        vector<GLenum> drawbuffers(2);
        drawbuffers[0] = GL_COLOR_ATTACHMENT0_EXT;
        drawbuffers[1] = GL_COLOR_ATTACHMENT1_EXT;
        fbo.SetDrawBuffers(drawbuffers);

        bool useDists = true;
        if(!tofIsPersp) {
            gpuDepthWarp.Init(*tofParam, useDists);
        } else {
            gpuDepthWarp.Init(*tofParamPersp, useDists);
        }

        gpuDepthWarp.SetRenderTarget(&fbo);
        gpuDepthWarp.SetTargetCameraParameters(*targetParam, *zNear, *zFar);

        if(hasTexture){
            if(tofParam != NULL)
                gpuDepthWarp.UseProjectiveTexturing(*tofParam);
            else if(tofParamPersp != NULL)
                gpuDepthWarp.UseProjectiveTexturing(*tofParamPersp);
            else return -1;
            gpuDepthWarp.UploadProjectiveTexture(texImage);
        }
        gpuDepthWarp.UploadDepthMap(tofImage);
        gpuDepthWarp.Draw();

        //    Camera<float> dispImg;
        //    dispTexture.CopyToImage(dispImg);
        //    dispImg.Flip();
        //    ImageIO::Save("disparity.mip",dispImg);

        depthTexture.CopyToImage(resultImg);
        resultImg.Flip();

        if (*translateToDepth) {
            float** z = resultImg.GetImageDataArray();
            Vector3<double> ray, p;
            double zNear_ = *zNear;
            double zFar_ = *zFar;
            for (unsigned int y=0; y<resultImg.GetHeight(); y++) {
                for (unsigned int x=0; x<resultImg.GetWidth(); x++) {
                    if (z[y][x] >= 1.0) z[y][x] = 0.0;
                    else if(z[y][x] != 0.0){
                        z[y][x] = zNear_*zFar_ / (zFar_ - (z[y][x])*(zFar_-zNear_));
                        targetProj.UnProjectToRayLocal(HomgPoint2D(x,y,1.0), p, ray);
                        ray = (z[y][x] / ray[2])*ray;
                        z[y][x] = ray.NormL2();
                    }
                }
            }
        }

        //    Camera<unsigned char> resultImage;
        //    resultTexture.CopyToImage(resultImage);
        //    resultImage.Flip();
        //    IOUtils::SaveCamera("texture.mip",resultImage);
    } catch (glfException& e) {
        std::cout << "Error: " << e.GetMessageString() << std::endl;
        pbuffer.Destroy();
        return -1;
    }



    pbuffer.Destroy();
#else
    BIASERR("Recompile BIAS with OpenGL to use GPU warping!");
#endif
  }//end else (*bSimple)

    ImageIO::Save(*targetImageName, resultImg);
}