OutputLensDistortion.cpp

#include <GLviewer/OutputAttachments/OutputLensDistortion.hh>
#include <Base/Image/Image.hh>
#include <Base/Image/ImageIO.hh>

using namespace std;
using namespace BIAS;

OutputLensDistortion::OutputLensDistortion(BIAS::glfRenderTarget *out) 
: GLProjectionOutputAttachment(out)
{
  InitShader_();
}

OutputLensDistortion::~OutputLensDistortion() {
  
}

int OutputLensDistortion::
InitShader_() {
  string vertexCode =
      "void main()\r\n"
      "{\r\n"
      "  gl_TexCoord[0] = gl_MultiTexCoord0;\r\n"
      "  gl_Position = ftransform();\r\n"
      "}\r\n";
  string fragmentCode = 
    "uniform sampler2D tex;\r\n"
    "uniform sampler2D dep;\r\n"
    "uniform float w;\r\n"
    "uniform float h;\r\n"
    "uniform sampler2D lookup;\r\n"
    ""
    " void main() {\r\n"
    ""
    "   vec2 indirect = texture2D(lookup, gl_TexCoord[0].st).st;\r\n"
    "   gl_FragColor = texture2D(tex, indirect);\r\n"   
    "   gl_FragDepth = texture2D(dep, indirect).r;\r\n"
    " }\r\n";

  return GLProjectionOutputAttachment::InitShader(vertexCode, fragmentCode);
}

void OutputLensDistortion::
CreateLookupTable(ProjectionParametersBase *pp) {
  ProjectionParametersPerspective *ppp = dynamic_cast<ProjectionParametersPerspective*>(pp);
  if (!ppp) {
    BIASERR("lens distortion only works on ProjectionParametersPerspective");
    BIASABORT;
  }
  KMatrix K, Kinv, idealK;
  K = ppp->GetK();
  Kinv = K.Invert();
  ppp->GetIdealK(idealK);
  unsigned int width, height;
  ppp->GetImageSize(width,height);
  Image<float> lookupTexture(width, height, 3);
  float* tex = lookupTexture.GetImageData();
  unsigned int c=0;  
  HomgPoint2D source(0,0,1);

  for (unsigned int y=0; y<height; y++) {
    for (unsigned int x=0; x<width; x++) {
      source[0] = x;
      source[1] = y;
      ppp->Undistort(source); 
      source = idealK * Kinv * source;
      source.Homogenize();
//      
      source[0] /= width;
      source[1] /= height;

      tex[c++] = source[0];
      tex[c++] = source[1];
      tex[c++] = source[2];
    }
  }
 
  distortionMap_.Create();
  distortionMap_.SetMinFilter(GL_NEAREST);
  distortionMap_.SetMagFilter(GL_NEAREST);
  distortionMap_.SetWrapT(GL_CLAMP);
  distortionMap_.SetWrapS(GL_CLAMP);
  distortionMap_.SetTextureNr(GL_TEXTURE2);
  distortionMap_.UploadImage(lookupTexture, GL_RGB32F_ARB);

}

int OutputLensDistortion::Draw_() {
  
    if(target_ == NULL){
        BIASWARN("Target is NULL, not drawing anything.");
        return -1;
    }
        
    Vector4<int> vp;
  glGetIntegerv(GL_VIEWPORT, vp.GetData());

  unsigned int imgWidth = vp[2] - vp[0];
  unsigned int imgHeight = vp[3] - vp[1];

  target_->ClearColorBuffer(ClearColor_[0],ClearColor_[1],ClearColor_[2],ClearColor_[3]);
  target_->ClearDepthBuffer();
  //  bind render target
  target_->Bind();
  shaderProg_.Bind();
  shaderProg_.SetUniform("tex", specialDisplayTex_.GetTextureNrInUniformFormat());
  shaderProg_.SetUniform("dep", specialDisplayDepth_.GetTextureNrInUniformFormat());
  shaderProg_.SetUniform("lookup", distortionMap_.GetTextureNrInUniformFormat());
  shaderProg_.SetUniform("w", (float)imgWidth);
  shaderProg_.SetUniform("h", (float)imgHeight);

  glMatrixMode(GL_PROJECTION);
  glPushMatrix();
  glLoadIdentity();
  gluOrtho2D(0, imgWidth, 0, imgHeight);
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glLoadIdentity();

  // quad to render texture on
  glPushAttrib(GL_ENABLE_BIT);
  glEnable(GL_TEXTURE_2D);
  specialDisplayTex_.BindTU();
  specialDisplayDepth_.BindTU();
  distortionMap_.BindTU();
  glBegin(GL_QUADS);
    glColor3f(1.0, 0.0, 0.0);
    glTexCoord2f(0.0, 0.0);
    glVertex2f(0.0, 0.0);

    glColor3f(0.0, 1.0, 0.0);
    glTexCoord2f(0.0, 1.0);
    glVertex2f(0.0, imgHeight);

    glColor3f(0.0, 0.0, 1.0);
    glTexCoord2f(1.0, 1.0);
    glVertex2f(imgWidth, imgHeight);

    glColor3f(1.0, 1.0, 1.0);
    glTexCoord2f(1.0, 0.0);
    glVertex2f(imgWidth, 0.0);
  glEnd();
  glFlush();
  glPopAttrib();
  glMatrixMode(GL_MODELVIEW);
  glPopMatrix();
  glMatrixMode(GL_PROJECTION);
  glPopMatrix();
  glUseProgram(0);

  GLF_THROW_ON_OPENGL_ERROR

  return 0;
}