GLProjectionParametersPerspective.cpp

#include <GLviewer/GLProjectionParametersPerspective.hh>
#include <GLviewer/Scenes/SceneBGImage.hh>
#include <OpenGLFramework/Base/glfMatrix.hh>

#define RAD_TO_DEG 57.29577951
#define DEG_TO_RAD 0.017453292

//#define SKEW

using namespace std;
using namespace BIAS;

GLProjectionParametersPerspective::
GLProjectionParametersPerspective() :
  GLProjectionParametersBase(),ProjectionParametersPerspective()
{
  ignoreDistortion_=false;
  distorter_ = -1;
}


GLProjectionParametersPerspective::
GLProjectionParametersPerspective(const BIAS::ProjectionParametersPerspective &ppp)
  : GLProjectionParametersBase(), ProjectionParametersPerspective(ppp)
{
  ignoreDistortion_=false;
  distorter_ = -1;
}

GLProjectionParametersPerspective&
GLProjectionParametersPerspective::
operator=(const GLProjectionParametersPerspective& glppp)
{
  this->ProjectionParametersPerspective::operator=(glppp);
  ignoreDistortion_= glppp.ignoreDistortion_;
  distorter_ = glppp.distorter_;
  outputAttachmentIdentifiers_ = glppp.outputAttachmentIdentifiers_;
  outputAttachmentOptions_ = glppp.outputAttachmentOptions_;
  outputAttachments_ = glppp.outputAttachments_;
  return *this;
}

GLProjectionParametersPerspective::
GLProjectionParametersPerspective(const GLProjectionParametersPerspective& glppp)
{
  this->ProjectionParametersPerspective::operator=(glppp);
  ignoreDistortion_= glppp.ignoreDistortion_;
  distorter_ = glppp.distorter_;
  outputAttachmentIdentifiers_ = glppp.outputAttachmentIdentifiers_;
  outputAttachmentOptions_ = glppp.outputAttachmentOptions_;
  outputAttachments_ = glppp.outputAttachments_;
}


#ifdef BIAS_HAVE_XML2
int GLProjectionParametersPerspective::
XMLRead (const std::string &Filename){
  //informScenesOfChange_ = true;
  ExtrinsicStateChanged_ = false;
  IntrinsicStateChanged_ = true;
  return ProjectionParametersPerspective::XMLRead(Filename);
}
int GLProjectionParametersPerspective::
XMLWrite (const std::string &Filename) const{
  return ProjectionParametersPerspective::XMLWrite(Filename);
}

int GLProjectionParametersPerspective::
Load    (const std::string& Filename){
  return XMLRead(Filename);
}
int GLProjectionParametersPerspective::
Save    (const std::string& Filename) const{
  return XMLWrite(Filename);
}
#endif


int GLProjectionParametersPerspective::
SetSimplePerspectiveCam(double fovyDEG, int imageWidth, int imageHeight)
{
    int viewport[4];
    viewport[0] = 0;
  viewport[1] = 0;

    if (imageWidth == -1 || imageHeight == -1)
    {
        viewport[2]=800;
        viewport[3]=600;
    }
    else
  {
    viewport[2] = imageWidth;
    viewport[3] = imageHeight;
  }

  double imgAspectRatio = (double) viewport[2] / (double) viewport[3];
  //use focallength of 1 since UpdateGLProjectionMatrix_ is taking care
  //of zNear_!
  double imageSceneHeight = 2.0 * tan(fovyDEG/2.0 * DEG_TO_RAD);
  double imageSceneWidth = imageSceneHeight * imgAspectRatio;

  KMatrix K;
  K.SetIdentity();
  //principle point
  K[0][2] = (double)viewport[2]/2.0 - 0.5;
  K[1][2] = (double)viewport[3]/2.0 - 0.5;
  //pixel scale
  K[0][0] = (double)viewport[2]/imageSceneWidth;
  K[1][1] = (double)viewport[3]/imageSceneHeight;

  if(!ProjectionParametersPerspective::PoseValid())
  {
    RMatrix R;
    R.SetIdentity();
    Vector3<double> C(0.0,0.0,0.0);
    ProjectionParametersPerspective::SetR(R);
    ProjectionParametersPerspective::SetC(C);
    ExtrinsicStateChanged_ = true;
  }

  ProjectionParametersPerspective::SetUndistortion(0.0,0.0,0.0,0.0);
  SetK(K);
  SetImageSize((unsigned int) viewport[2],
               (unsigned int) viewport[3]);

  IntrinsicStateChanged_ = true;
  informScenesOfChange_ = true;
  return 0;
}


void GLProjectionParametersPerspective::
SetIntrinsics(BIAS::ProjectionParametersBase *proj)
{
 ProjectionParametersPerspective* ppp
     = dynamic_cast<ProjectionParametersPerspective*>(proj);

 if (!ppp) {
   BIASERR("Not a perspective projection!")
 }
 else {
   const Pose pose = GetPose(); // backup of extrinsics
   ProjectionParametersPerspective::operator=(*ppp);
   SetPose(pose);
   InitShaders_();
   informScenesOfChange_ = true;
   ExtrinsicStateChanged_ = false;
   IntrinsicStateChanged_ = true;
 }
}


void GLProjectionParametersPerspective::
Set(BIAS::ProjectionParametersBase *proj)
{
  ProjectionParametersPerspective* ppp
      = dynamic_cast<ProjectionParametersPerspective*>(proj);

  if (!ppp) {
    BIASERR("Not a perspective projection!")
  }
  else {
    ProjectionParametersPerspective::operator=(*ppp);
    InitShaders_();
    informScenesOfChange_ = true;
    ExtrinsicStateChanged_ = true;
    IntrinsicStateChanged_ = true;
  }
}


int GLProjectionParametersPerspective::
SetGLMatrices(bool forceUpdate)
{
  if(ExtrinsicStateChanged_ || forceUpdate)
    if(UpdateGLModelViewMatrix_()!=0)
      return -1;

  unsigned int imgWidth, imgHeight;
  GetImageSize(imgWidth, imgHeight);
  //  cout<<" GLProjectionParametersPerspective SetGLMatrices:"<<imgWidth<<" "<<imgHeight<<endl;

  if(IntrinsicStateChanged_ || forceUpdate) {
    if(UpdateGLProjectionMatrix_(0,0,imgWidth, imgHeight)!=0)
      return -1;
  }

  glMatrixMode(GL_PROJECTION);
  glLoadMatrixd(GLProjectionMatrix_.GetData());
  glMatrixMode(GL_MODELVIEW);
  glLoadMatrixd(GLModelViewMatrix_.GetData());
  return 0;
}



void GLProjectionParametersPerspective::
SetClearColor(const BIAS::Vector4<float>& cc)
{
  ClearColor_ = cc;
}


void GLProjectionParametersPerspective::
Rescale_(float scale)
{
  //cout << "*** GLPPP::Rescale_() with scale = " << scale << endl;
  ProjectionParametersPerspective::Rescale(scale);
  InitShaders_();
}


int GLProjectionParametersPerspective::
UpdateGLProjectionMatrix_(unsigned int x0, unsigned int y0,
                          unsigned int width, unsigned int height)
{
  //  cout<<"UpdateGLProjectionMatrix_ start image size: "<<width<<","<<height<<endl;
  Vector3<double> clippingWindowUL, clippingWindowLR;

    double kc1, kc2, kc3, kc4;
    GetUndistortion(kc1, kc2, kc3, kc4);
    if((kc1==0.0 && kc2==0.0 && kc3==0.0 && kc4==0.0) || ignoreDistortion_)
    {

#ifndef SKEW
      Vector3<double> pos;
      UnProjectLocal(HomgPoint2D(x0-0.5,y0-0.5), pos, clippingWindowUL);
      UnProjectLocal(HomgPoint2D(x0+width-0.5,y0+height-0.5), pos, clippingWindowLR);
#endif
      distort_ = false;

    }
    else
    {
      if (distorter_ < 0) InitShaders_();
      KMatrix idealK;
      unsigned int fullW, fullH;
      GetImageSize(fullW, fullH);
      SetIdealImageSize(width, height);

#ifndef SKEW
      GetIdealK(idealK);
      GetIdealImageSize(width, height);
#endif

      if(/*x0 != 0 || y0 != 0 ||*/ fullW!=width || fullH!=height)
      {
        //we must allow to move the position of the viewport or we might get a black border
        //over the image - which will result in unaligned mouse and screen coordinates
        //x0 = 0 and y0 = 0 are gl viewport coordinates which describe the LOWER left corners
        //of the viewport.
        BIASERR("distortion rendering is not implemented to work with image "
            "cut outs!"<<endl<<"x0: "<<x0<<" y0:"<<y0<<" fullW: "<<fullW
            <<" widht:"<<width  <<" fullH: "<<fullH<<" height: "<<height);
        return -1;
      }

#ifndef SKEW
      Vector3<double> pos;
      ProjectionParametersPerspective distortDummy = *this;
      distortDummy.SetImageSize(width, height);
      distortDummy.SetK(idealK);
      distortDummy.SetUndistortion(0.0,0.0,0.0,0.0);

      distortDummy.UnProjectLocal(HomgPoint2D(x0-0.5,y0-0.5), pos, clippingWindowUL);
      distortDummy.UnProjectLocal(HomgPoint2D(x0+width-0.5, y0+height-0.5), pos, clippingWindowLR);
#endif
    }
    //calculate the near clipping plane in world coord metric

#ifndef SKEW
    clippingWindowUL *= (zNear_/clippingWindowUL[2]);
    clippingWindowLR *= (zNear_/clippingWindowLR[2]);
#endif

    glPushAttrib(GL_TRANSFORM_BIT);
    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();

#ifndef SKEW
    glFrustum(clippingWindowUL[0], clippingWindowLR[0],
                //calculate clip window in our coord system
                //transform into gl coords by flipping around x-axis
              -clippingWindowLR[1], -clippingWindowUL[1],
              zNear_, zFar_);
#endif
#ifdef SKEW
    glfMatrix projectionMatrix;
    ProjectionParametersPerspective* param =
      dynamic_cast<ProjectionParametersPerspective*>(GetMyselfAsProjectionParameterBase());
    projectionMatrix.MakeProjectionMatrixNew(*param, zNear_, zFar_,
                                              x0, y0, width, height,
                                              distort_);
    projectionMatrix.Load();
#endif


    glGetDoublev( GL_PROJECTION_MATRIX, GLProjectionMatrix_.GetData() );
    glPopMatrix();
    glPopAttrib(); //GL_TRANSFORM_BIT

    IntrinsicStateChanged_ = false;
    //    cout<<"Set:zNear_, zFar_"<<zNear_<<","<<zFar_<<endl;
    //    cout<<"UpdateGLProjectionMatrix_ x0:"<<x0<<","<<y0<<","<< width<<","<<height<<endl;
    return 0;
}


int GLProjectionParametersPerspective::
Draw_(std::vector<SceneBase *>& scenes,
      BIAS::glfFramebufferObject* theTarget)
{
  //////////////////////////////
  try{
    BeginDraw_(theTarget);
  }catch(glfException& e){
    BIASERR("GL Error in Draw_() : " << e.GetMessageString());
    return -1;
  }
  /////////////////////////////
  if(scenes.size() > 0){
    for (unsigned int i=0; i<scenes.size(); i++) {
      if(informScenesOfChange_){
        scenes[i]->UpdateCameraRelatedState();
      }
      scenes[i]->Render();
    }
    if(useStencilBuffer_) ClearDepthAndAlpha_();
  }//end of rendering regular scenes
     //fragment from VisionN is in EndDraw_
  /////////////////////////////
  
  try{
    EndDraw_(theTarget);
  }catch(glfException& e){
    BIASERR("GL Error in Draw_() : " << e.GetMessageString());
    return -1;
  }
  /////////////////////////////
  return 0;
}

void GLProjectionParametersPerspective::
ClearDepthAndAlpha_()
{
    //set all pixels
    //stencil test passes to zfar
    //enable stencil test
    glPushAttrib(GL_DEPTH_BUFFER_BIT | GL_TEXTURE_BIT |GL_STENCIL_BUFFER_BIT);
    glEnable(GL_STENCIL_TEST);
    glEnable(GL_DEPTH_TEST);
    glDepthMask(GL_TRUE);
    glStencilFunc(GL_EQUAL, 1, (GLuint)(~0));
    glStencilOp(GL_KEEP, GL_KEEP, GL_KEEP);
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glLoadIdentity();
    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    glDepthFunc(GL_ALWAYS); //<-------- overwrite everything not stenciled!
    glOrtho(-1.0, 1.0, -1.0, 1.0, -1.0, 1.0);
    glColorMask(0,0,0,1); //clear alpha channel or background image blending will use alpha channel
    float s=1;
    glDisable(GL_TEXTURE_2D);
    glBegin(GL_QUADS);
    glColor4f(0,1,1,0); //clear alpha channel with 0 rgb can be anything
    glVertex3f(-s, s, -s);
    glVertex3f(-s, -s, -s);
    glVertex3f(s, -s, -s);
    glVertex3f(s, s, -s);
    glEnd();
    glColorMask(1,1,1,1);
    glDepthFunc(GL_LESS);
    glMatrixMode(GL_PROJECTION);
    glPopMatrix();
    glMatrixMode(GL_MODELVIEW);
    glPopMatrix();
    glPopAttrib();
    //------------------------
    glDisable(GL_STENCIL_TEST);
}

int GLProjectionParametersPerspective::
DrawWithMatchedParamsAndViewport_(int viewport[4],
        int imageWidth,int imageHeight,
        std::vector<SceneBase*>& scenes)
{
    /*
  cout << "\nentering GLPPP::DrawWithMatchedParamsAndViewport_() with image ("
       << imageWidth << " x " << imageHeight << ")" << endl;
  BIAS::Vector4<int> tmp;
  glGetIntegerv(GL_VIEWPORT, tmp.GetData());
  cout << "entering GLPPP::DrawWithMatchedParamsAndViewport_(): GLViewport is " << tmp[0]
       << ", " << tmp[1]
       << ", " << tmp[2]
       << ", " << tmp[3] << endl;
     */

    int res =0;
    if (AutoViewingMatchBehaviour_ == AdaptToCroppedImage) {
        if(lastFullGLViewport_[2] == viewport[2] &&
                lastFullGLViewport_[3] == viewport[3])
        {
            res = Draw_(scenes);
        }
        else
        {
            double decisionScaleWidth =  (double)viewport[2] / (double)imageWidth;
            double decisionScaleHeight = (double)viewport[3] / (double)imageHeight;
            double decisionScale;

            if(decisionScaleWidth>decisionScaleHeight)
                decisionScale = 1.0/decisionScaleWidth;
            else
                decisionScale = 1.0/decisionScaleHeight;


            //force new image dimension (don't use rescale - it fails because of rounding errors)
            SetImageSize(viewport[2],viewport[3]);
            SetIdealImageSize(viewport[2],viewport[3]);
            //scale principal point
            double cx,cy;
            GetPrincipal(cx,cy);
            SetPrincipal(cx/decisionScale,cy/decisionScale);
            //scale focal length
            double f;
            //TODO adjust skew ...
            GetFocalLength(f);
            SetFocalLengthAndAspect(f/decisionScale,(double)viewport[2]/(double)viewport[3]);
            GLProjectionParametersBase::Rescale(1.0); //create new K

            for(unsigned int i=0; i<4; i++)
                lastFullGLViewport_[i] = viewport[i];

            SetGLViewport_(lastFullGLViewport_[0],lastFullGLViewport_[1],
                    lastFullGLViewport_[2],lastFullGLViewport_[3]);

            IntrinsicStateChanged_=true;
            informScenesOfChange_ = true;

            if (UpdateGLProjectionMatrix_(lastFullGLViewport_[0],
                    lastFullGLViewport_[1],lastFullGLViewport_[2],
                    lastFullGLViewport_[3])!=0) BIASABORT;

            InitShaders_();

            res = Draw_(scenes);

        }
        return res;
    } //if (AutoViewingMatchBehaviour_ == AdaptToCroppedImage)
    else
        return GLProjectionParametersBase::DrawWithMatchedParamsAndViewport_( viewport, imageWidth, imageHeight,scenes);
}



void GLProjectionParametersPerspective::
InitShaders_()
{
  if (distorter_ == -1) {
    
    double kc1, kc2, kc3, kc4;
    GetUndistortion(kc1, kc2, kc3, kc4);
    if((kc1==0.0 && kc2==0.0 && kc3==0.0 && kc4==0.0) || ignoreDistortion_)
      return;
    
    AppendOutputAttachment(GLProjectionOutputAttachment::LensDistortion);

    for (unsigned int i=0;i<outputAttachmentIdentifiers_.size();i++) {
      if (outputAttachmentIdentifiers_[i] == GLProjectionOutputAttachment::LensDistortion) {
        distorter_ = i;
      }
    }
  }
}


double GLProjectionParametersPerspective::
Z2Depth_(const int x, const  int y, const double z) const
{
  double depth;
  if (z >= 1.0){
    depth = 0.0;
  } else {
    BIASASSERT(zFar_!=zNear_);
    BIASASSERT((zFar_ - (double)z*(zFar_-zNear_))!=0);
    depth = zNear_*zFar_ / (zFar_ - (double)z*(zFar_-zNear_));
    // the OpenGL depth is merely the z coordinate in the camera
    // coordinate system, while the MIP depth is given by the distance
    // between the point and the center of projection. Compute the MIP depth
    // from the nvidia depth
    ProjectionParametersBase *ppb = GetMyselfAsProjectionParametersBase();
    BIASASSERT(ppb);
    Vector3<double> pl, pos;
    ppb->UnProjectLocal(BIAS::HomgPoint2D(x, y, 1.0), pos, pl);
    BIASASSERT(!Equal(pl[2], 0.0));
    pl /= pl[2];
    depth *= pl.NormL2();
  }
  return depth;
}