CylinderMapping.cpp

// BIAS includes
#include "CylinderMapping.hh"


using namespace BIAS;
using namespace std;


static double toDeg(const double rad) {
  return (rad / M_PI * 180.0);
}

static double toRad(const double deg) {
  return (M_PI / 180.0 * deg);
}


static void rotateY(BIAS::Vector3<double>& v, const double rad) {
  v.Set(v[2] * sin(rad) + v[0] * cos(rad),
        v[1],
        v[2] * cos(rad) - v[0] * sin(rad));
}


CylinderMapping::CylinderMapping()
{ 
  Reset();
}

void CylinderMapping::Reset()
{
  px_=-1;          // principal x
  py_=-1;          // principal y
  
  focalX_=100;
  focalY_=80;

  camAngleX_=-1;   // camera opening angle in x-direction
  camAngleY_=-1;   // camera opening angle in y-direction
}

void CylinderMapping::setFocalLength(const double fx, const double fy) {
  focalX_=fx;
  focalY_=fy;
}


void CylinderMapping::setPrincipalPoint(const double in_px_, 
                                      const double in_py_) {
  px_ = in_px_;
  py_ = in_py_;
}


int CylinderMapping::GetSourceCoordinates_(const BIAS::HomgPoint2D& sink, 
                                          BIAS::HomgPoint2D& source)
                                          const {

  double camAngleX;//camAngleX;
  camAngleX = toDeg(atan(width_ * 0.5 / focalX_)) * 2.0;  
  //camAngleY = toDeg(atan(height_ * 0.5 / focalY_)) * 2.0;
  
  double px=px_, py=py_;
  if (px==-1) px = (width_-1) / 2.0;
  if (py==-1) py = (height_-1) / 2.0;
  
  BIAS::HomgPoint2D normsink(sink[0] /  width_,
                             sink[1] / height_);

  BIAS::Vector3<double> v1(0.0, 0.0, 1.0);
  rotateY(v1, toRad(-camAngleX / 2.0));

  double origWidth = fabs(v1[0] * (1.0 / v1[2])) * 2.0;

  rotateY(v1, toRad(sink[0] / (double) width_ * camAngleX));
  v1[1] = normsink[1] - 0.5;

  double lambda = 1.0 / v1[2];
  source[0] = lambda * v1[0];
  source[1] = lambda * v1[1];

  source[0] = (source[0] / origWidth + 0.5) * width_;
  source[1] = (source[1] + 0.5) * height_;

  return 0;
}