PMDWarp.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 "PMDWarp.hh"
#include <OpenGLFramework/Base/glfException.hh>
#include <Base/Debug/TimeMeasure.hh>

using namespace BIAS;
using namespace std;

bool timing=false;
TimeMeasure timer;

void
PMDWarp::Init(const ProjectionParametersPerspective& pppd,
              bool targetParametersHaveDistortion)
{
  pmdParams_ = pppd;
  pmdParams_.GetImageSize(depthImageWidth_, depthImageHeight_);

  pmdDepthImage_.Create();
  pmdDepthImage_.SetMinFilter(GL_NEAREST);
  pmdDepthImage_.SetMagFilter(GL_NEAREST);
  pmdDepthImage_.SetWrapS(GL_CLAMP);
  pmdDepthImage_.SetWrapT(GL_CLAMP);
  batch_.SetTexture(&pmdDepthImage_, 0);

  depthBufferMode_.SetDepthTest(true);
  batch_.SetDepthBufferMode(&depthBufferMode_);

  batch_.SetProjectionMatrix(&Projection_);
  batch_.SetModelViewMatrix(&modelView_);

  shaderProgram_.Create();
  batch_.SetShaderProgram(&shaderProgram_);

  batch_.SetViewport(&viewport_);

  targetParametersHaveDistortion_ = targetParametersHaveDistortion; 

  SetupShadersDefault_();

  SetupDefaultPrimitiveRendering_();
  
}

glfBatch&
PMDWarp::GetBatch()
{
  return batch_;
}

BIAS::glfDepthBufferMode&
PMDWarp::GetDepthBufferMode()
{
  return depthBufferMode_;
}

/** Upload the pmd depth image to the graphics card.
 **/
void
PMDWarp::UploadDepthMap(const BIAS::Image<float>& pmdDepthImage)
{
  pmdDepthImage_.UploadImage(pmdDepthImage);
}

void
PMDWarp::UploadDepthMap(const BIAS::ImageBase& pmdDepthImage)
{
  pmdDepthImage_.UploadImage(pmdDepthImage);
}

void
PMDWarp::UseProjectiveTexturing(const ProjectionParametersPerspective& ppp)
{
  projectiveTexture_.Create();
  projectiveTexture_.SetMinFilter(GL_LINEAR);
  projectiveTexture_.SetMagFilter(GL_LINEAR);
  projectiveTexture_.SetWrapS(GL_CLAMP);
  projectiveTexture_.SetWrapT(GL_CLAMP);
  batch_.SetTexture(&projectiveTexture_, 1);

  ProjectionParametersPerspective relativePPP;
  relativePPP = ppp;
  Pose relativePose;
  relativePose.BecomeRelativeTransform(ppp.GetPose(), pmdParams_.GetPose());
  relativePPP.SetPose(relativePose);
  //cout<<"Projective Pose:"<<relativePose<<endl;

  //do not flip the texture because the image has been flipped
  //implicitly by uploading it to graphics card
  textureMatrix_.MakeTextureMatrixNew(relativePPP, false);
  batch_.SetTextureMatrix(&textureMatrix_, 1);
  //cout<<"K:\n"<<relativePPP.GetK()<<endl;
  //cout<<"TextureMatrix:\n"<<textureMatrix_<<endl;
  SetupShadersProjectiveTexturing_();
  
  shaderProgram_.SetUniform("C", relativePPP.GetC());
}

void
PMDWarp::UploadProjectiveTexture(const BIAS::Image<unsigned char>& projectiveTexture)
{
  projectiveTexture_.UploadImage(projectiveTexture);
}

void
PMDWarp::SetRenderTarget(const glfRenderTarget* renderTarget)
{
  batch_.SetRenderTarget(renderTarget);
}

void
PMDWarp::SetViewportSize(unsigned int width, unsigned int height)
{
  viewport_.SetSize(width, height);
  viewport_.SetOrigin(0, 0);
}

//is also used to determine viewport 
void
PMDWarp::SetTargetCameraParameters(
    const ProjectionParametersPerspective& relativePpp, 
    double zNear,double zFar)
{
  targetParams_ = relativePpp;
  modelView_.MakeViewMatrixNew(targetParams_);
  Projection_.MakeProjectionMatrixNew(targetParams_, zNear, zFar);

  if(targetParametersHaveDistortion_) {
    double k0,k1,t0,t1;
    targetParams_.GetUndistortion(k0, k1, t0,t1);    
    shaderProgram_.SetUniform("k0", k0);
    shaderProgram_.SetUniform("k1", k1);
  }

  unsigned int width, height;
  targetParams_.GetImageSize(width, height);
  viewport_.SetSize(width, height);
  viewport_.SetOrigin(0, 0);

}

void
PMDWarp::SetTargetCameraParameters(BIAS::Matrix4x4<double> modelViewMatrix,
    BIAS::Matrix4x4<double> projectionMatrix, unsigned int width,
    unsigned int height)
{
  modelView_.Make(modelViewMatrix);
  Projection_.Make(projectionMatrix);
  viewport_.SetSize(width, height);
  viewport_.SetOrigin(0, 0);
}

void
PMDWarp::SetTargetCameraParametersToGlobal(bool global)
{
  if (global)
    {
      batch_.SetProjectionMatrix(&globalModelViewNProjection_);
      batch_.SetModelViewMatrix(&globalModelViewNProjection_);
    }
  else
    {
      batch_.SetProjectionMatrix(&Projection_);
      batch_.SetModelViewMatrix(&modelView_);
      batch_.SetDepthBufferMode(&depthBufferMode_);
    }
}

void
PMDWarp::SetViewportToGlobal(bool global)
{
  if (global)
    {
      batch_.SetViewport(&globalViewport_);
    }
  else
    {
      batch_.SetViewport(&viewport_);
    }
}

void
PMDWarp::ChangeZClipping(double zNear, double zFar)
{
  Projection_.MakeProjectionMatrix(targetParams_, zNear, zFar);
}

//renders the result into the forseen output
void
PMDWarp::Draw()
{   
    
    if(timing){     
        timer.Start();
  }
  if (defaultRendering_)
    {
      unsigned int numIndicesPerStrip = depthImageWidth_ * 2;
      unsigned int first = 0;
      for (unsigned int y = 0; y < depthImageHeight_ - 1; y++)
        {
          batch_.SetRange(first, numIndicesPerStrip);
          batch_.Draw();
          first += numIndicesPerStrip;
        }
    }
  else
    {
      BIASERR("no rendering implemented!\n");
    }
  
  if(timing){
    timer.Stop();
    cout<<"PMDWarp::Draw:"; timer.PrintRealTime();
    timer.Reset();
    timer.Start();
  }  
}

PMDWarp::PMDWarp() :
  defaultRendering_(true)
{
}

PMDWarp::~PMDWarp()
{
}

void
PMDWarp::SetupDefaultPrimitiveRendering_()
{

  glfVertexFormat vertexFormat;
  vertexFormat.AddAttribute(glfVertexFormat::ATTRIB_POSITION, GL_FLOAT, 4);
  vertexFormat.AddAttribute(glfVertexFormat::ATTRIB_TEXCOORD, 0, GL_FLOAT, 2);
  //vertexFormat.AddAttribute(VertexFormat::ATTRIB_COLOR, GL_FLOAT, 3);

  //const int numComponenents = 9;//4 for vertex 2 for tex coord 3 for color
  const int numComponenents = 6;//4 for vertex 2 for tex coord
  float* vertexData = new float[depthImageWidth_ * depthImageHeight_
                                * numComponenents];

  unsigned int count = 0;
  for (unsigned int y = 0; y < depthImageHeight_; y++)
    {
      for (unsigned int x = 0; x < depthImageWidth_; x++)
        {
          HomgPoint2D imgPoint(x, y);
          Vector3<double> ray, p;
          pmdParams_.UnProjectLocal(imgPoint, p, ray);

          if(ray.NormL2() > 1e-12)  ray.Normalize();
          //else BIASERR("error");
          
          vertexData[count++] = static_cast<float> (ray[0]);
          vertexData[count++] = static_cast<float> (ray[1]);
          vertexData[count++] = static_cast<float> (ray[2]);
          vertexData[count++] = 1.0;

          vertexData[count++] = (static_cast<float> (x) + 0.5)
            / static_cast<float> (depthImageWidth_);
          vertexData[count++] = (static_cast<float> (y) + 0.5)
            / static_cast<float> (depthImageHeight_);

          //           vertexData[count++] =
          //             (static_cast<float>(x)+0.5)/static_cast<float>(depthImageWidth_);
          //           vertexData[count++] =
          //             (static_cast<float>(y)+0.5)/
          //             static_cast<float>(depthImageHeight_);
          //           vertexData[count++] = 1.0;
        }
    }

  vertices_.Create(depthImageWidth_ * depthImageHeight_, vertexFormat,
                   vertexData);
  batch_.SetVertexBuffer(&vertices_);
  delete[] vertexData;

  //************** INDICES ***********************
  //creating coordinates for triangle strips!
  //each image rows, except for the first and last, are contained twice
  //a triangle strip consists of 2*width vertices
  // and height-1 strips have to be rendered
  GLuint* triangleIndices = new GLuint[depthImageWidth_ * (depthImageHeight_
                                                           - 1) * 2];
  unsigned int numTriangleIndex = 0;
  //
  for (unsigned int y = 0; y < depthImageHeight_ - 1; y++)
    {
      for (unsigned int x = 0; x < depthImageWidth_; x++)
        {
          /*         v1
           *      i0--- i2
           *       |   / |
           *       |  /  |
           *       i1    i3
           */
          triangleIndices[numTriangleIndex++] = y * depthImageWidth_ + x;//up
          triangleIndices[numTriangleIndex++] = (y + 1) * depthImageWidth_ + x;//below
        }
    }
  elements_.Create(depthImageWidth_ * (depthImageHeight_ - 1) * 2,
                   GL_UNSIGNED_INT, GL_TRIANGLE_STRIP, triangleIndices);
  batch_.SetElementBuffer(&elements_);
  delete[] triangleIndices;

  defaultRendering_ = true;

}

void
PMDWarp::SetupShadersDefault_()
{

  stringstream vertexProgramCode;
  vertexProgramCode<<
    "uniform sampler2D pmdDepthImage;\n"
    "uniform float scale = 1.0;"
    "uniform float imageWidth = 900;"
    "uniform float dist_from_cam = 2000;"
    "uniform float disp_for_dist_from_cam = 255.0;"
    "varying float disp;\n"
    ""
    "\n"; 
  if(targetParametersHaveDistortion_) {
    vertexProgramCode<<
      "uniform float k0;\n"
      "uniform float k1;\n"
      "\n";
  }
  
  vertexProgramCode<<
    "void main() {\n"  
    "  float depth = texture2D(pmdDepthImage, gl_MultiTexCoord0.st).r;\n"
    "  if(depth == 0) depth = 0.9;\n"
    "  vec4 depthAdjustedVertex = gl_Vertex * depth;\n"
    "  depthAdjustedVertex.w = 1.0; \n"
    "  depthAdjustedVertex = gl_ModelViewMatrix * depthAdjustedVertex;\n";

  //add distortion here:
  if(targetParametersHaveDistortion_) {
    vertexProgramCode<<
      "float normalizedRad = (depthAdjustedVertex[0]*depthAdjustedVertex[0]);\n "
      "normalizedRad += (depthAdjustedVertex[1]*depthAdjustedVertex[1]);\n"
      "normalizedRad /= (depthAdjustedVertex[2]*depthAdjustedVertex[2]);\n"
      "float scale = 1.0f + (k0 + k1*normalizedRad)*normalizedRad;\n"
      "depthAdjustedVertex[0] = scale*depthAdjustedVertex[0];\n"
      "depthAdjustedVertex[1] = scale*depthAdjustedVertex[1];\n";
  }
  
  vertexProgramCode<<   
    "vec4 tmp = gl_ModelViewProjectionMatrix * vec4(imageWidth,0.0,0.0,0.0);"
    "vec4 t = vec4(abs(((dist_from_cam*disp_for_dist_from_cam)/tmp[0])), 0.0,0.0,0.0);"
    "vec4 proj_t = gl_ModelViewProjectionMatrix * t;"
    ""
    ""
    "vec4 pos = gl_ProjectionMatrix * depthAdjustedVertex;\n"
    "disp = scale*abs(proj_t[0]/pos[3])*imageWidth;"
    ""
    "gl_Position = pos;";
  vertexProgramCode<<
    "  gl_TexCoord[0] = gl_MultiTexCoord0;\n"
    "}";
  
  string fragmentProgramCode = 
    "varying float disp;"
    ""    
    "void main() {"
    "gl_FragData[0] = gl_TexCoord[0];"
    "gl_FragData[1] = vec4(disp);"

    "}";
  
  
  string geometryProgramCode = ""
                              "#extension GL_EXT_geometry_shader4 : enable\n"
                              ""
                              ""
                              "void main() {\n"
                              ""
                              ""
                              "vec4 v0 = gl_PositionIn[0];"
                              "vec4 v1 = gl_PositionIn[1];"
                              "vec4 v2 = gl_PositionIn[2];"
                              ""
                              "float dmax = max ( max ( distance(v0,v1), distance(v1,v2) ), distance(v0,v2) );"
                              ""
                              "if (dmax < 300) {"
                              ""
                              "gl_Position = v0;"
                              "gl_TexCoord[0] = gl_TexCoordIn[0][0];"
                              "EmitVertex();"
                              "gl_Position = v1;"
                              "gl_TexCoord[0] = gl_TexCoordIn[1][0];"
                              "EmitVertex();"
                              "gl_Position = v2;"
                              "gl_TexCoord[0] = gl_TexCoordIn[2][0];"
                              "EmitVertex();"
                              ""
                              "} else {"
                              ""
                              "}"
                              ""
                              ""
                              "}\n";

  vertexProgram_.Create(GL_VERTEX_SHADER, vertexProgramCode.str());  
  fragmentProgram_.Create(GL_FRAGMENT_SHADER, fragmentProgramCode);
  geometryProgram_.Create(GL_GEOMETRY_SHADER_EXT, geometryProgramCode);

  shaderProgram_.AttachShader(vertexProgram_);
  shaderProgram_.AttachShader(fragmentProgram_);
  shaderProgram_.AttachShader(geometryProgram_);

  glProgramParameteriEXT(shaderProgram_.GetProgramId(),
                         GL_GEOMETRY_VERTICES_OUT_EXT, 3);
  glProgramParameteriEXT(shaderProgram_.GetProgramId(),
                         GL_GEOMETRY_INPUT_TYPE_EXT, GL_TRIANGLES);
  glProgramParameteriEXT(shaderProgram_.GetProgramId(),
                         GL_GEOMETRY_OUTPUT_TYPE_EXT, GL_TRIANGLE_STRIP);

  shaderProgram_.Link();
  shaderProgram_.SetUniform("pmdDepthImage", 0);
}

void PMDWarp::SetDispForDistanceAndScale(const float& dist_from_cam,
          const float& disp, const float& scale)
{
    shaderProgram_.SetUniform("dist_from_cam", dist_from_cam);
    shaderProgram_.SetUniform("disp_for_dist_from_cam", disp);
    shaderProgram_.SetUniform("scale", scale);
}

void
PMDWarp::ExecuteLinearHypothesesAssignment(
    const BIAS::ProjectionParametersPerspective& relativePpp, double zNear,
    double zFar, float N, int output)
{
  //set the target rendering params
  SetTargetCameraParameters(relativePpp, zNear, zFar);

  //calculate the constants needed
  float FN = zNear * zFar;
  float FMinusN = zFar - zNear;
  float relativeHypoSpacing = FMinusN / N;

  stringstream fragmentProgramStream;
  fragmentProgramStream << "uniform float zNear;" << "uniform float zFar;"
      << "uniform float FMinusN;" << "uniform float FN;" << "uniform float N;"
      << "uniform float D;";//relativeHypoSpacing


  fragmentProgramStream << "void main() {"
      << " float z = FN/(zFar - gl_FragCoord.z*FMinusN);"
      << " float r = (z-zNear)/D;" << " float l = floor(r);" //lower representation
      << " float u = ceil(r);" //upper representation
      << " float w = r - l;" //relative distance
      << " gl_FragData[" << output << "].r = l;" << " gl_FragData[" << output
      << "].g = u;" << " gl_FragData[" << output << "].b = w;" << "}";

  try
    {
      fragmentProgram_.Create(GL_FRAGMENT_SHADER, fragmentProgramStream.str());
    }
  catch (glfException& e)
    {

      std::cout << "Error: " << fragmentProgram_.GetInfoLog() << std::endl;
      throw(e);
    }

  //set appropriate constants
  try
    {
      shaderProgram_.Link();
    }
  catch (glfException& e)
    {
      std::cout << "Error: " << shaderProgram_.GetInfoLog() << std::endl;
      throw(e);
    }
  shaderProgram_.SetUniform("zNear", zNear);
  shaderProgram_.SetUniform("zFar", zFar);
  shaderProgram_.SetUniform("FN", FN);
  shaderProgram_.SetUniform("FMinusN", FMinusN);
  shaderProgram_.SetUniform("D", relativeHypoSpacing);
  shaderProgram_.SetUniform("N", N);
}

void
PMDWarp::SetupShadersProjectiveTexturing_()
{
  cout<<"projective texturing...\n";
  stringstream vertexProgramCode;
  vertexProgramCode<< 
    "uniform sampler2D pmdDepthImage;" 
    "uniform float scale = 1.0;"
    "uniform float imageWidth = 900;"
    "uniform float dist_from_cam = 2000;"
    "uniform float disp_for_dist_from_cam = 255.0;"
    "varying float disp;\n"
    ""
    "\n";
  if(targetParametersHaveDistortion_) {
    vertexProgramCode<<
      "uniform float k0;\n"
      "uniform float k1;\n"
      "\n";
  }
  vertexProgramCode<< 
    "void main() {"
    "  float depth = texture2D(pmdDepthImage, gl_MultiTexCoord0.st).r;\n"  
    "  if(depth == 0) depth = 0.9;\n"
    "  vec4 depthAdjustedVertex = gl_Vertex * depth;\n"
    "  depthAdjustedVertex.w = 1.0;\n"
    "  vec4 origdepthAdjustedVertex = depthAdjustedVertex;"
    "  depthAdjustedVertex = gl_ModelViewMatrix * depthAdjustedVertex;\n";

  //add distortion here:
 if(targetParametersHaveDistortion_) {
    vertexProgramCode<<
      "float normalizedRad = (depthAdjustedVertex[0]*depthAdjustedVertex[0]);\n "
      "normalizedRad += (depthAdjustedVertex[1]*depthAdjustedVertex[1]);\n"
      "normalizedRad /= (depthAdjustedVertex[2]*depthAdjustedVertex[2]);\n"
      "float scale = 1.0f + (k0 + k1*normalizedRad)*normalizedRad;\n"
      "depthAdjustedVertex[0] = scale*depthAdjustedVertex[0];\n"
      "depthAdjustedVertex[1] = scale*depthAdjustedVertex[1];\n";
  }

  vertexProgramCode<<
        "vec4 tmp = gl_ModelViewProjectionMatrix * vec4(imageWidth,0.0,0.0,0.0);"
        "vec4 t = vec4(abs(((dist_from_cam*disp_for_dist_from_cam)/tmp[0])), 0.0,0.0,0.0);"
        "vec4 proj_t = gl_ModelViewProjectionMatrix * t;"
        ""
        ""
    "vec4 pos = gl_ProjectionMatrix * depthAdjustedVertex;\n"
    "disp = scale*abs(proj_t[0]/pos[3])*imageWidth;"
    ""
      "gl_Position = pos;"  
    //use original depthadjusted vertex, without application of modelview matrix!!
    "gl_TexCoord[0] = gl_TextureMatrix[1] * origdepthAdjustedVertex;"
    "}";
 
  string fragmentProgramCode = 
    "uniform sampler2D projectiveTexture;"
    ""
    "varying float disp;"
    ""
    "void main() {"
    "gl_FragData[0] = texture2DProj(projectiveTexture, gl_TexCoord[0]);"
    "gl_FragData[1] = vec4(disp);"  
    "}";
  
  vertexProgram_.Create(GL_VERTEX_SHADER, vertexProgramCode.str());
  fragmentProgram_.Create(GL_FRAGMENT_SHADER, fragmentProgramCode);
  
  //don't use already attached!
  //shaderProgram_.AttachShader(vertexProgram_);
  //shaderProgram_.AttachShader(fragmentProgram_);
  shaderProgram_.Link();
  shaderProgram_.SetUniform("pmdDepthImage", 0);
  shaderProgram_.SetUniform("projectiveTexture", 1);
}