ProjectionParametersBufferedRay.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 "Geometry/ProjectionParametersBufferedRay.hh"
#include <Base/Common/CompareFloatingPoint.hh>
#include <Geometry/ProjectionParametersFactory.hh>
#include <MathAlgo/PolynomialSolve.hh>
#include <MathAlgo/Minpack.hh>

using namespace BIAS;
using namespace std;

ProjectionParametersBufferedRay::
ProjectionParametersBufferedRay(const ProjectionParametersBase *ppb)
  : ProjectionParametersBase()
{
  if (ppb)
    ppb_ = ppb->Clone();
  else
    ppb_ = NULL;
  unprojectMappingIgnoreDistortion_ = NULL;
  unprojectMappingDistortion_ = NULL;
  mask_ = false;
  if(!UpdateFromProjection_())
  {
    BIASERR("ProjectionParameters are NULL");
  }
}

ProjectionParametersBufferedRay::
ProjectionParametersBufferedRay(const ProjectionParametersBufferedRay &ppbr)
{
  *this = ppbr;
  if (ppbr.ppb_)
    ppb_ = ppbr.ppb_->Clone();
  else
    ppb_ = NULL;
  unprojectMappingIgnoreDistortion_ = NULL;
  unprojectMappingDistortion_ = NULL;
  mask_ = ppbr.mask_;
  if(!UpdateProjection_())
  {
    BIASERR("ProjectionParameters are NULL");
  }
}

bool ProjectionParametersBufferedRay::Distort(BIAS::HomgPoint2D& point2d) const
{
  return ppb_->Distort(point2d);
}


bool ProjectionParametersBufferedRay::Undistort(BIAS::HomgPoint2D& point2d) const
{
  return ppb_->Undistort(point2d);
}

const bool ProjectionParametersBufferedRay::
DoIntrinsicsDiffer(const ProjectionParametersBase* p) const
{
  return ppb_->DoIntrinsicsDiffer(ppb_);
}

HomgPoint2D ProjectionParametersBufferedRay::
ProjectLocal(const Vector3<double>& point, bool IgnoreDistortion) const
{
  return ppb_->ProjectLocal(point, IgnoreDistortion);
}


int ProjectionParametersBufferedRay::
ProjectLocal(const Vector3<double>& point, HomgPoint2D &p2d,
             bool IgnoreDistortion) const
{
  return ppb_->ProjectLocal(point, p2d, IgnoreDistortion);
}

HomgPoint3D ProjectionParametersBufferedRay::
UnProjectToImagePlane(const HomgPoint2D& pos, const double& depth,
                      bool IgnoreDistortion) const
{
  return ppb_->UnProjectToImagePlane(pos, depth, IgnoreDistortion);
}

void ProjectionParametersBufferedRay::
UnProjectLocal(const HomgPoint2D& pos, Vector3<double>& origin,
               Vector3<double>& direction, bool IgnoreDistortion) const
{
#ifdef BIAS_DEBUG
  if(pos[0] < 0.0)
    BIASERR("negative index: "<<pos[0]);
  if(pos[1] < 0.0)
    BIASERR("negative index: "<<pos[1]);
  if((int)(pos[0]+0.5) >= (int)width_)
    BIASERR("index out of bound: "<<(int)(pos[0]+0.5));
  if((int)(pos[1]+0.5) >= (int)height_)
    BIASERR("index out of bound: "<<(int)(pos[1]+0.5));
#endif
  if(IgnoreDistortion)
  {
    direction = unprojectMappingIgnoreDistortion_[(int)(pos[0]+0.5)+
                                             ((int)(pos[1]+0.5))*width_];
  }
  else
  {
    direction = unprojectMappingDistortion_[(int)(pos[0]+0.5)+((int)(pos[1]+0.5))*width_];
  }
  origin = Vector3<double>(0.0);
}

Vector3<double> ProjectionParametersBufferedRay::
UnProjectToPointLocal(const HomgPoint2D& pos, const double& depth,
                      bool IgnoreDistortion) const
{
  Vector3<double> x, p;
  UnProjectLocal(pos, p, x, IgnoreDistortion);
  x *= depth;
  return x;
}

#ifdef BIAS_HAVE_XML2

int ProjectionParametersBufferedRay::
XMLGetClassName(std::string& TopLevelTag, double& Version) const
{
  TopLevelTag = "ProjectionParametersBufferedRay";
  Version = 0.1;
  return 0;
}

int ProjectionParametersBufferedRay::
XMLOut(const xmlNodePtr Node, XMLIO& XMLObject) const
{
  xmlNodePtr theNode;
  string TopLevelName;
  double Version;
  ProjectionParametersBase::XMLGetClassName(TopLevelName, Version);
  theNode = XMLObject.addChildNode(Node, TopLevelName);
  XMLObject.addAttribute(theNode, "Version", Version);
  ProjectionParametersBase::XMLOut(theNode, XMLObject);

  // Add node for wrapped projection parameters
  xmlNodePtr childNode = XMLObject.addChildNode(Node, "WrappedProjection"); 
  if (ppb_) {
    ppb_->XMLGetClassName(TopLevelName, Version);
    theNode = XMLObject.addChildNode(childNode, TopLevelName);
    XMLObject.addAttribute(theNode, "Version", Version);
    ppb_->XMLOut(theNode, XMLObject);
  }

  XMLObject.addAttribute(Node, "Mask", mask_);

  return 0;
}

int ProjectionParametersBufferedRay::
XMLIn(const xmlNodePtr Node, XMLIO& XMLObject)
{
  string TopLevelName;
  double Version;
  ProjectionParametersBase::XMLGetClassName(TopLevelName, Version);

  xmlNodePtr childNode = XMLObject.getChild(Node, TopLevelName);
  if (!childNode) {
    BIASERR("Error in XML, tag " << TopLevelName << " not found!");
    return -1;
  }
  if (ProjectionParametersBase::XMLIn(childNode, XMLObject) != 0) return -1;

  // Read wrapped projection parameters
  if (ppb_) delete ppb_; 
  childNode = XMLObject.getChild(Node, "WrappedProjection");
  if (childNode) {
    childNode = XMLObject.getFirstChild(childNode);
    if (childNode) {
      ProjectionParametersFactory factory;
      string nodename = XMLObject.getNodeName(childNode);
      ppb_ = factory.Create(nodename);
      if (ppb_->XMLIn(childNode, XMLObject) != 0) return -1;
    }
  }

  mask_ = XMLObject.getAttributeValueBool(Node, "Mask");

  // Update wrapped projection parameters
  if(!UpdateProjection_())
  {
    BIASERR("ProjectionParameters are NULL");
  }
  
  return 0;
}

#endif //BIAS_HAVE_XML2

const bool ProjectionParametersBufferedRay::UpdateProjection_()
{
  if (ppb_ == NULL) return false;
  ppb_->SetImageSize(width_, height_);
  ppb_->SetPrincipal(principalX_, principalY_);
  ppb_->SetAspectratio(aspectratio_);
  if (CValid_ && QValid_) {
    ppb_->SetQC(GetQ(), GetC());
  } else if (CValid_) {
    ppb_->SetQ(GetQ());
    ppb_->InvalidatePose();
    ppb_->SetC(GetC());
  } else if (QValid_) {
    ppb_->SetC(GetC());
    ppb_->InvalidatePose();
    ppb_->SetQ(GetQ());
  } else {
    ppb_->SetQC(GetQ(), GetC());
    ppb_->InvalidatePose();
  }
  ppb_->SetCov(GetCov());
  ppb_->SetIdentifier(identifier_);
  //ppb_->ustIgnoreDistortion_ = ustIgnoreDistortion_;
  //ppb_->ustNormalize_ = ustNormalize_;
  //ppb_->ustTransformIntoImage_ = ustTransformIntoImage_;

  return UpdateBuffer_();
}

const bool ProjectionParametersBufferedRay::UpdateFromProjection_()
{
  if (ppb_ == NULL) return false;
  unsigned int w, h;
  double px, py;
  ppb_->GetImageSize(w, h);
  ppb_->GetPrincipal(px, py);
  SetImageSize(w, h);
  SetPrincipal(px, py);
  SetAspectratio(ppb_->GetAspectratio());
  SetPose(ppb_->GetPose());
  if (!ppb_->PoseValid()) InvalidatePose();
  SetIdentifier(ppb_->GetIdentifier());
  //ustIgnoreDistortion_ = ppb_->ustIgnoreDistortion_;
  //ustNormalize_ = ppb_->ustNormalize_;
  //ustTransformIntoImage_ = ppb_->ustTransformIntoImage_;

  return UpdateBuffer_();
}

const bool ProjectionParametersBufferedRay::UpdateBuffer_()
{
  if (ppb_ == NULL) return false;
  Vector3<double> tmp;
  const double halfX = width_/2.0, halfY = height_/2.0;
  const BIAS::Vector3<double> zero(0.0, 0.0, 0.0);
  double rx, ry, r;
  if (unprojectMappingIgnoreDistortion_)
    delete[] unprojectMappingIgnoreDistortion_;
  if (unprojectMappingDistortion_)
    delete[] unprojectMappingDistortion_;
  unprojectMappingIgnoreDistortion_ = new Vector3<double>[width_*height_];
  unprojectMappingDistortion_ = new Vector3<double>[width_*height_];
  for (unsigned int y = 0; y < height_; y++)
  {
    for (unsigned int x = 0; x < width_; x++)
    {
      if(mask_)
      {
        rx = (x-halfX)/halfX;
        ry = (y-halfY)/halfY;
        r = sqrt(rx*rx+ry*ry);
      }
      else
      {
        r = 0.5;
      }
      if (r <= 1.0)
      {
        Vector3<double> p;
        ppb_->UnProjectLocal(HomgPoint2D(x,y), p, tmp, true);
        BIASASSERT(fabs(tmp.Length() - 1.0) <= std::numeric_limits<double>::epsilon() ||
                   fabs(tmp.Length()) <= std::numeric_limits<double>::epsilon())
        unprojectMappingIgnoreDistortion_[x+y*width_] = tmp;
        ppb_->UnProjectLocal(HomgPoint2D(x,y), p, tmp, false);
        BIASASSERT(fabs(tmp.Length() - 1.0) <= std::numeric_limits<double>::epsilon() ||
                   fabs(tmp.Length()) <= std::numeric_limits<double>::epsilon())
        unprojectMappingDistortion_[x+y*width_] = tmp;
      }
      else
      {
        unprojectMappingIgnoreDistortion_[x+y*width_] = zero;
        unprojectMappingDistortion_[x+y*width_] = zero;
      }
    }
  }
  return true;
}