ProjectionParametersCylindric.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/ProjectionParametersCylindric.hh" 
#include <Base/Common/CompareFloatingPoint.hh>

using namespace std;
using namespace BIAS;


bool BIAS::ProjectionParametersCylindric::
Distort(BIAS::HomgPoint2D& point2d) const
{
  BIASERR("unfinished");
  BIASABORT;
  return false;
}

bool BIAS::ProjectionParametersCylindric::
Undistort(BIAS::HomgPoint2D& point2d) const
{
  BIASERR("unfinished");
  BIASABORT;
  return false;
}

void BIAS::ProjectionParametersCylindric::
UpdateQuasiK_()
{
  //normal coordinates are mapped into image as follows: 
  //(x, phi) -> (  x * focallengthX_   + principalX_, 
  //             phi * focallengthPhi_ + principalY_)  
  focallengthX_ = double(width_)/(xMax_-xMin_);
  //xMin_ is mapped to first column
  principalX_ = (-1.0)*xMin_*focallengthX_;
  focallengthPhi_ = double(height_)/(phiMax_-phiMin_);
  //phiMin_ is mapped to first row
  principalY_ = (-1.0)*phiMin_*focallengthPhi_;
}


void BIAS::ProjectionParametersCylindric::
UpdateFromQuasiK_() 
{
  xMin_ = (-1.0)*principalX_/focallengthX_;
  xMax_ = double(width_)/focallengthX_ + xMin_;
  phiMin_ = (-1.0)*principalY_/focallengthPhi_;
  phiMax_ = double(height_)/focallengthPhi_ + phiMin_;
}


HomgPoint2D BIAS::ProjectionParametersCylindric::
ProjectLocal(const BIAS::Vector3<double> &point,
             bool IgnoreDistortion) const
{
//if (!IgnoreDistortion) {
//  BIASERR("Distortion is not implemented for cylindric projections!");
//  BIASABORT;
//}

  HomgPoint2D res;
  if (Equal(point.NormL2(), 0.0)) {
    res.SetZero();
    return res;
  }

  // Translate and rotate point vector according to the cylinder's orientation
  Vector3<double> p;
  AxisRotationInv_.MultVec(point, p);
  if (Equal(p[1], 0.0) && Equal(p[2], 0.0)) {
    return HomgPoint2D(0, 0, 0);
  }  
  if (UseAxisOffset_) {
    // Calculate 3D intersection point of vector with cylinder hull
    double a = p[2]*AxisOffsetInv_[2] + p[1]*AxisOffsetInv_[1];
    double b = AxisOffsetInv_[2]*AxisOffsetInv_[2] +
               AxisOffsetInv_[1]*AxisOffsetInv_[1];
    double c = p[2]*p[2] + p[1]*p[1];
    double scale = a/c + sqrt((a*a / c - b + 1.0) / c);
    p = scale * p - AxisOffsetInv_;
  } else {
    // Calculate 3D intersection point of vector with cylinder hull
    double scale = 1.0/sqrt(p[1]*p[1] + p[2]*p[2]);
    p.MultIP(scale);
  }
  Vector2<double> xPhi;
  ProjectToNormalizedCoords_(p, xPhi);

  // Calculate 2D coordinates of intersection point in cylinder hull image
  res[0] = xPhi[0]*focallengthX_+principalX_;
  res[1] = xPhi[1]*focallengthPhi_+principalY_;
  res[2] = 1.0;
  return res;
}


int ProjectionParametersCylindric::
ProjectLocal(const Vector3<double>& point, HomgPoint2D &p2d,
             bool IgnoreDistortion) const
{ 
  p2d = ProjectLocal(point, IgnoreDistortion);
  return 0;
}


HomgPoint3D ProjectionParametersCylindric::
UnProjectToImagePlane(const HomgPoint2D& pos,
                      const double& depth,
                      bool IgnoreDistortion) const 
{
  Vector3<double> d, p;
  UnProjectLocal(pos, p, d, IgnoreDistortion);
  double radius = sqrt(d[2]*d[2] + d[1]*d[1]);
  if (Equal(radius, 0.0))
    return HomgPoint3D(0, 0, 0, 0);
  d *= depth / radius;
  if (Equal(d.NormL2(), 0.0))
    return HomgPoint3D(0, 0, 0, 0);
  HomgPoint3D local(d);
  return Pose_.LocalToGlobal(local);
}


void BIAS::ProjectionParametersCylindric::
UnProjectLocal(const HomgPoint2D &pos, Vector3<double>& pointOnRay,
          Vector3<double>& direction, bool IgnoreDistortion) const
{
  // Normalize unprojected 3D point (resulting from 2D point pos) to length 1
  direction = UnProjectToCylinderPoint(pos, IgnoreDistortion);
  direction.Normalize();
  pointOnRay = Vector3<double>(0.0);
}


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


Vector3<double> BIAS::ProjectionParametersCylindric::
UnProjectToCylinderPoint(const HomgPoint2D &pos,
                         bool IgnoreDistortion) const
{
  // Calculate 3D point on hull of cylinder with radius 1 relative to
  // the camera center, where 2D point given lies on cylinder hull image.
  Vector3<double> p;
  double phi;
  p[0] = (pos[0]-principalX_)/focallengthX_;
  phi = (pos[1]-principalY_)/focallengthPhi_;
  p[1] = sin(phi);
  p[2] = cos(phi);
  // Rotate resulting vector according to the cylinder's orientation
  Vector3<double> res;
  AxisRotation_.MultVec(p, res);
  // Translate according to cylinder's offset
  if (UseAxisOffset_) {
    res += AxisOffset_;
  }
  return res;
}


const bool BIAS::ProjectionParametersCylindric::
DoIntrinsicsDiffer(const ProjectionParametersBase* p) const
{
  if (ProjectionParametersBase::DoIntrinsicsDiffer(p)) {
    return true;
  }
  const ProjectionParametersCylindric* ppc = 
    dynamic_cast<const ProjectionParametersCylindric*>(p);
  if (ppc == NULL) {
    return true;
  }
  if (focallengthPhi_ != ppc->focallengthPhi_) {
    return true;
  }
  if (focallengthX_ != ppc->focallengthX_) {
    return true;
  }
  return false;
}


bool BIAS::ProjectionParametersCylindric::
DoesPointProjectIntoImageLocal(const BIAS::Vector3<double> &localP, 
                               HomgPoint2D &p, bool IgnoreDistortion) const
{
  p = ProjectLocal(localP, IgnoreDistortion);
  p.Homogenize(); //neccessary ?
  return (p[0] >= 0.0 && p[1] >= 0.0 && 
          p[0] <= (double)(width_-1) && p[1] <= (double)(height_-1));
}


void BIAS::ProjectionParametersCylindric::
SetCylinderRotation(const Vector3<double> &rotAxis, double rotAngle,
                    const ProjectionCylinderAxisEnum axisType)
{
  BIAS::Quaternion<double> rotation;
  rotation.SetValueAsAxisRad(rotAxis, rotAngle);
  SetCylinderRotation(rotation, axisType);
}


void BIAS::ProjectionParametersCylindric::
SetCylinderRotation(const Quaternion<double> &rotation,
                    const ProjectionCylinderAxisEnum axisType)
{
  AxisRotation_ = rotation;
  if (axisType == AXIS_PARALLEL) {
    BIAS::Quaternion<double> axisRot;
    axisRot.SetValueAsAxisRad(Vector3<double>(0.0, -1.0, 0.0), 0.5*M_PI);
    AxisRotation_.Mult(axisRot);
  } else if (axisType != AXIS_ORTHOGONAL) {
    BIASERR("ProjectionParametersCylindric::SetCylinderRotation(): "
            "Invalid axis type given!");
  }
  AxisRotationInv_ = AxisRotation_.Inverse();
  AxisRotationInv_.MultVec(AxisOffset_, AxisOffsetInv_);
}


void BIAS::ProjectionParametersCylindric::
SetCylinderType(const ProjectionCylinderAxisEnum axisType)
{
  if (axisType == AXIS_ORTHOGONAL) {
    AxisRotation_.SetValueAsAxisRad(Vector3<double>(0.0, 1.0, 0.0), 0.0);
    AxisRotationInv_ = AxisRotation_.Inverse();
    AxisRotationInv_.MultVec(AxisOffset_, AxisOffsetInv_);
  } else if (axisType == AXIS_PARALLEL) {
    AxisRotation_.SetValueAsAxisRad(Vector3<double>(0.0, -1.0, 0.0), 0.5*M_PI);
    AxisRotationInv_ = AxisRotation_.Inverse();
    AxisRotationInv_.MultVec(AxisOffset_, AxisOffsetInv_);
  } else {
    BIASERR("ProjectionParametersCylindric::SetCylinderType(): "
            "Invalid axis type given!");
  }
}


void BIAS::ProjectionParametersCylindric::
SetCylinderOffset(const Vector3<double> &offset)
{
  AxisOffset_ = offset;
  AxisRotationInv_.MultVec(AxisOffset_, AxisOffsetInv_);
  UseAxisOffset_ = !(Equal(AxisOffset_.NormL2(), 0.0));
}


/** @brief Takes the difference of the cylinder centers weighted by
           the cylinders' orientation difference to model the
           view difference. Minimal difference 0 is achived, if
           both cylinder coincide. Difference increases with cylinder
           center distance |M0-M1| and cylinder axis angle difference.
  */
double BIAS::ProjectionParametersCylindric::
ViewDifference(const ProjectionParametersBase* pPPB) const
{
  const ProjectionParametersCylindric *pPPC = 
    dynamic_cast<const ProjectionParametersCylindric* >(pPPB);
  // different projection model has a very different view
  if (pPPC == NULL) {
    BIASERR("View difference for different projection models requested !");
    return DBL_MAX;
  }
  Vector3<double> C0 = pPPC->GetC();
  Vector3<double> C1 = GetC();
  RMatrix R0 = pPPC->GetR();
  RMatrix R1 = GetR();
  Vector3<double> V0 = R0*pPPC->AxisRotation_.MultVec(Vector3<double>(0,0,1));
  Vector3<double> V1 = R1*AxisRotation_.MultVec(Vector3<double>(0,0,1));
  Vector3<double> M0 = C0 + V0 * 0.5*(pPPC->xMax_ - pPPC->xMin_);
  Vector3<double> M1 = C1 + V1 * 0.5*(xMax_ - xMin_);
  double phi = acos(V0.ScalarProduct(V1));
  return (M0-M1).NormL2()*(1.0 - fabs(sin(phi)));
}


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


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

  // write "focal length" scale factors to xml
  XMLObject.addAttribute(Node, "FocallengthX", focallengthX_);
  XMLObject.addAttribute(Node, "FocallengthPhi", focallengthPhi_);

  // write cylinder rotation to xml
  //XMLObject.addAttribute(Node, "CylinderAxisRotation",
  //                       BIAS::Vector<double>(AxisRotation_).GetSTLVec());
  // deprecated xml format for rotations, quaternion should be used instead!
  double angle;
  BIAS::Vector3<double> axis;
  AxisRotation_.GetAxisAngle(axis, angle);
  XMLObject.addAttribute(Node, "CylinderAxisRotAxis",
                         BIAS::Vector<double>(axis).GetSTLVec());
  XMLObject.addAttribute(Node, "CylinderAxisRotAngle", angle);

  // write cylinder translation to xml
  XMLObject.addAttribute(Node, "CylinderAxisTranslation",
                         BIAS::Vector<double>(AxisOffset_).GetSTLVec());
  
  return 0; 
}


int ProjectionParametersCylindric::
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 "focal length" scale factors from xml
  focallengthX_ = XMLObject.getAttributeValueDouble(Node, "FocallengthX");
  focallengthPhi_ = XMLObject.getAttributeValueDouble(Node, "FocallengthPhi");
  
  // read cylinder rotation from xml
  xmlAttrPtr quatAttr = XMLObject.getAttributeByName(Node, "CylinderAxisRotation");
  if (quatAttr) {
    std::vector<double> quat = XMLObject.getAttributeValueVecDbl(quatAttr);
    BIASASSERT(quat.size() == 4);
    SetCylinderRotation(Quaternion<double>(quat[0], quat[1], quat[2], quat[3]));
  } else {
    // deprecated xml format, quaternion should be used instead!
    std::vector<double> axis =
      XMLObject.getAttributeValueVecDbl(Node, "CylinderAxisRotAxis");
    BIASASSERT(axis.size() == 3);
    double angle = XMLObject.getAttributeValueDouble(Node, "CylinderAxisRotAngle");
    SetCylinderRotation(Vector3<double>(axis[0], axis[1], axis[2]), angle);
//#ifdef BIAS_DEBUG
//    Quaternion<double> quat;
//    quat.SetValueAsAxisRad(Vector3<double>(axis[0], axis[1], axis[2]), angle);
//    BIASWARN("Deprecated cylindric projection parameter XML format found!\n"
//             << "Use quaternion " << quat << " instead of angle/axis representation "
//             << "for orientation (" << angle << " rad around axis "
//             << Vector3<double>(axis[0], axis[1], axis[2]) << ")!");
//#endif
  }

  // read cylinder translation from xml
  std::vector<double> offset =
    XMLObject.getAttributeValueVecDbl(Node, "CylinderAxisTranslation");
  BIASASSERT(offset.size() == 3);
  SetCylinderOffset(Vector3<double>(offset[0], offset[1], offset[2]));

  UpdateFromQuasiK_();
  return 0;
}

#endif // BIAS_HAVE_XML2