ProjectionParametersSphericalSimple.cpp

#include "Geometry/ProjectionParametersSphericalSimple.hh"

#include <Base/Debug/Exception.hh>

using namespace std;
using namespace BIAS;


ProjectionParametersSphericalSimple::
ProjectionParametersSphericalSimple()
  : ProjectionParametersBase(), phiOffset_(0.0), thetaOffset_(0.0),
    dPhi_(0.0), dTheta_(0.0)
{}

ProjectionParametersSphericalSimple::
ProjectionParametersSphericalSimple(const double phiOffs,
                                    const double thetaOffs,
                                    const double dPhi,
                                    const double dTheta,
                                    const unsigned int width,
                                    const unsigned int height)
  : ProjectionParametersBase(width, height),
    phiOffset_(phiOffs), thetaOffset_(thetaOffs), dPhi_(dPhi), dTheta_(dTheta)
{
  // deal with periodicy of phi and theta
  // phi must be in range between -pi and +pi
  // theta must be in range [0, pi]
  const double pi = M_PI;
  const double two_pi = 2. * M_PI;
  while (phiOffset_<-pi) { phiOffset_ += two_pi; }
  while (phiOffset_>pi) { phiOffset_ -= two_pi; }
  while (thetaOffset_<0.) { thetaOffset_ += two_pi; }
  while (thetaOffset_>two_pi) { thetaOffset_ -= two_pi; }
}


/** @brief calculates the projection of a point
        in the local camera coordinate system
        to a pixel in the image plane of the camera

        In the simplest case perspective pinhole projection x = K * point
        where point is transformed of X using  point = (R^T | -R^T C) X */
HomgPoint2D ProjectionParametersSphericalSimple::
ProjectLocal(const Vector3<double>& point, bool IgnoreDistortion) const
{
  HomgPoint2D res;
  ProjectLocal(point, res, IgnoreDistortion);

  return res;
}


int ProjectionParametersSphericalSimple::
ProjectLocal(const Vector3<double>& point, HomgPoint2D &p2d,
             bool IgnoreDistortion) const
{
  if (fabs(point.NormL2())<1e-6) { // point coincides with camera center
    p2d.Set(0., 0., 0.);
    return -1;
  }
  Vector2<double> pixel;
  Local2Pixel(point, pixel);
  p2d.Set(pixel[0], pixel[1], 1.0);
  return 0;
}



/** @brief calculates the unit size viewing ray from the camera center
        (in the camera coordinate system) which belongs to the image position
        pos e.g. pos=principal point results in (0,0,1)=optical axis */
void ProjectionParametersSphericalSimple::
UnProjectLocal(const HomgPoint2D& pos, Vector3<double>& pointOnRay,
          Vector3<double>& direction, bool IgnoreDistortion) const
{
  if (pos.IsAtInfinity()) {
    BEXCEPTION("cannot unproject point at infinity "<<pos);
  }
  Vector2<double> pix(pos[0]/pos[2], pos[1]/pos[2]);
  Pixel2Local(pix, direction);
  pointOnRay = Vector3<double>(0.0);
}


void ProjectionParametersSphericalSimple::
Rescale(double ratio, const double offset)
{
  ProjectionParametersBase::Rescale(ratio, offset);
  dPhi_ *= ratio;
  dTheta_ *= ratio;
}

void ProjectionParametersSphericalSimple::Rescale(unsigned int width, unsigned int height){

  unsigned int oldwidth = width_;
  ProjectionParametersBase::Rescale(width, height);
  dPhi_ *= ((double)oldwidth/(double)width_);
  dTheta_ *= ((double)oldwidth/(double)width_);
}


/* map points from image onto unit diameter image plane in 3D. */
HomgPoint3D  ProjectionParametersSphericalSimple::
UnProjectToImagePlane(const HomgPoint2D& pos, const double& depth,
                      bool IgnoreDistortion) const
{
  HomgPoint3D res(0,0,0,0);
  Vector3<double> localv, p;
  UnProjectLocal(pos, p, localv, IgnoreDistortion);
  double r = localv.NormL2();
  if(Equal(r,0.0))
  {
    return res;
  }
  localv*= depth/r;
  if(Equal(localv.NormL2(), 0.0))
    return res;
  res = HomgPoint3D(localv);
  res[3] = 1.0;
  return Pose_.LocalToGlobal(res);
}


bool ProjectionParametersSphericalSimple::
DoesPointProjectIntoImageLocal(const Vector3<double>& localX,
                               HomgPoint2D& x, bool IgnoreDistortion) const
{
   x = ProjectLocal(localX, IgnoreDistortion);
   return((x[0]>=0.0)&&(x[0]+1.0<=width_)&&
          (x[1]>=0.0)&&(x[1]+1.0<=height_));
}


#ifdef BIAS_HAVE_XML2

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


int ProjectionParametersSphericalSimple::
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);

  XMLObject.addAttribute(Node,"ThetaOffset", thetaOffset_);
  XMLObject.addAttribute(Node,"PhiOffset", phiOffset_);
  XMLObject.addAttribute(Node,"DTheta", dTheta_);
  XMLObject.addAttribute(Node,"DPhi", dPhi_);

  return 0;
}


int ProjectionParametersSphericalSimple::
XMLIn(const xmlNodePtr Node, XMLIO& XMLObject)
{
  string TopLevelName;
  double Version;
  ProjectionParametersBase::XMLGetClassName(TopLevelName, Version);
  if (XMLObject.getAttributeByName(Node, "Version")==NULL) {
    BIASERR("error getting \"Version\" attribute of \n");
    return -1;
  }
  double VersionInFile = XMLObject.getAttributeValueDouble(Node, "Version");
  if (VersionInFile<Version){
    BEXCEPTION("Cannot read old version "<<VersionInFile
               <<" of ProjectionParametersSphericalSimple");
  }

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


  if (XMLObject.getAttributeByName(Node, "ThetaOffset")==NULL)
    return -1;
  thetaOffset_  = XMLObject.getAttributeValueDouble(Node, "ThetaOffset");

  if (XMLObject.getAttributeByName(Node, "PhiOffset")==NULL)
    return -1;
  phiOffset_  = XMLObject.getAttributeValueDouble(Node, "PhiOffset");

  if (XMLObject.getAttributeByName(Node, "DTheta")==NULL)
    return -1;
  dTheta_  = XMLObject.getAttributeValueDouble(Node, "DTheta");
  if (XMLObject.getAttributeByName(Node, "DPhi")==NULL)
    return -1;
  dPhi_  = XMLObject.getAttributeValueDouble(Node, "DPhi");

  // deal with periodicy of phi and theta
  // phi must be in range between -pi and +pi
  // theta must be in range [0, pi]
  const double pi = M_PI;
  const double two_pi = 2. * M_PI;
  while (phiOffset_<-pi) { phiOffset_ += two_pi; }
  while (phiOffset_>pi) { phiOffset_ -= two_pi; }
  while (thetaOffset_<0.) { thetaOffset_ += two_pi; }
  while (thetaOffset_>two_pi) { thetaOffset_ -= two_pi; }

  R_ = GetR();

  return 0;
}

#endif // BIAS_HAVE_XML2


void ProjectionParametersSphericalSimple::
SetPoseParametrization(const PoseParametrization& pp)
{
  ProjectionParametersBase::SetPoseParametrization(pp);
  R_ = Pose_.GetR();
}


void ProjectionParametersSphericalSimple::
SetQ(const BIAS::Quaternion<double>& Q)
{
  ProjectionParametersBase::SetQ(Q);
  R_ = Pose_.GetR();
}


void ProjectionParametersSphericalSimple::
SetQC(const BIAS::Quaternion<double>& Q, const BIAS::Vector3<double>& C)
{
  ProjectionParametersBase::SetQC(Q, C);
  R_ = Pose_.GetR();
}


void ProjectionParametersSphericalSimple::
SetPose(const BIAS::Pose pose)
{
  ProjectionParametersBase::SetPose(pose);
  R_ = Pose_.GetR();
}

void ProjectionParametersSphericalSimple::
SetR(const BIAS::RMatrix& R)
{
  ProjectionParametersBase::SetR(R);
  R_ = R;
}

void ProjectionParametersSphericalSimple::
ValidatePose()
{
  ProjectionParametersBase::ValidatePose();
  R_ = ProjectionParametersBase::GetR();
}