EpipolarLine.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 "EpipolarLine.hh"

#include <Base/Debug/Debug.hh>
#include <Base/ImageUtils/ImageDraw.hh>
#include <Base/ImageUtils/Bresenham.hh>
#include <cmath>
#include <cfloat>

#define MY_DBL_PRECISION 1e-10

#include <Base/Common/BIASpragma.hh>

using namespace BIAS;
using namespace std;

// the following is used in DetermineIntersectionsAndEpipoleDistances_
int CompIntersect(const void *left, const void *right);

ostream& operator<<(ostream& os, const enum TEdge edge)
{
  switch(edge){
  case TopEdge:     os << "top";    break;
  case RightEdge:    os << "right";    break;
  case BottomEdge:    os << "bottom";    break;
  case LeftEdge:    os << "left";    break;
  case InvalidEdge:    os << "invalid";    break;
  }
  return os;
}


EpipolarLine::EpipolarLine() : HomgLine2D(), Debug()
{
  //Width_ = Height_ = 0;
  minx_ = maxx_ = miny_ = maxy_ = 0.0;

  Angle_ = DeltaR_ = RMinLine_ = RMaxLine_ = 0.0;
  // set all HomgPoints2D to (0,0,1)
  CloseIntersection_.Set (0.0, 0.0);
  FarIntersection_ = Current_ = TransformedStart_ = TransformedEnd_
    = TransformedCurrent_ = Epipole_ = PInfinity_ = CloseIntersection_;
  dx_ = dy_ = 0.0;
}

EpipolarLine::EpipolarLine(const EpipolarLine& line)
  :HomgLine2D(line)
{
  Epipole_ = line.GetCopyOfEpipole();
  EpipolePosition_ = line.GetEpipolePosition();
  Type_ = line.GetLineType();

  //Width_ = line.GetImageWidth();
  //Height_ = line.GetImageHeight();
  line.GetImageBoundaries(minx_,miny_,maxx_, maxy_);

  Angle_ = line.GetAngle();
  FarIntersection_ = line.GetCopyOfFarIntersectionWithImageBorder();
  CloseIntersection_ = line.GetCopyOfCloseIntersectionWithImageBorder();
  FarEdge_ = line.GetFarEdge();
  CloseEdge_ = line.GetCloseEdge();
  RMinLine_ = line.GetRMin();
  RMaxLine_ = line.GetRMax();
  dx_ = line.GetDx();
  dy_ = line.GetDy();
}


EpipolarLine& EpipolarLine::operator=(const EpipolarLine& line)
{
  (*this).HomgLine2D::operator=(line);
  Epipole_ = line.GetCopyOfEpipole();
  EpipolePosition_ = line.GetEpipolePosition();
  Type_ = line.GetLineType();
  //  Width_ = line.GetImageWidth();
  //Height_ = line.GetImageHeight();
  line.GetImageBoundaries(minx_,miny_,maxx_,maxy_);
  Angle_ = line.GetAngle();
  FarIntersection_ = line.GetCopyOfFarIntersectionWithImageBorder();
  CloseIntersection_ = line.GetCopyOfCloseIntersectionWithImageBorder();
  FarEdge_ = line.GetFarEdge();
  CloseEdge_ = line.GetCloseEdge();
  RMinLine_ = line.GetRMin();
  RMaxLine_ = line.GetRMax();
  dx_ = line.GetDx();
  dy_ = line.GetDy();
  return *this;
}

EpipolarLine::EpipolarLine(const Vector3<double>& vec)
  :HomgLine2D(vec)
{}

EpipolarLine::~EpipolarLine ()
{}

void
EpipolarLine::GetNextLine(bool clockwise, EpipolarLine& nextline,
                          double dist)
{
  double dx, dy;
  HomgPoint2D newpoint;

  Homogenize();
  //    cerr <<"slope: "<<slope<<"\tinv. slope: "<<bool2string(invslope)
  //         <<"\tdr: "<<GetDeltaR)<<"\ttype: "<<GetLineType()
  //         <<"\tangle: "<<GetAngle()<<"\tclockwise: "<<bool2string(clockwise)
  //         <<endl;

  dx = dist*fabs(data_[0]);
  dy = dist*fabs(data_[1]);

  if (data_[0] * data_[1] < 0)
    dx *= -1;

  if (fabs(GetAngle())>M_PI_2){
    dx *= -1;
    dy *= -1;
  }

  if (!clockwise){
    dx *= -1;
    dy *= -1;
  }

  newpoint = GetFarIntersectionWithImageBorder();

  // maybe newpoint.Homogenize() ?
  newpoint[0] += dx;
  newpoint[1] += dy;

  nextline.Init(minx_, miny_, maxx_, maxy_,
                GetEpipole(),
                newpoint, GetEpipolePosition());

  BIASDOUT(D_EL_NEXTLINE,"dx: "<<dx<<"\tdy: "<<dy<<"\tnewpoint: ("
          <<newpoint[0]<<", "<<newpoint[1]<<")   far int.: ("
          <<nextline.GetFarIntersectionWithImageBorder()[0]<<", "
          <<nextline.GetFarIntersectionWithImageBorder()[1]<<")  angle: "
          <<nextline.GetAngle());
}


int
EpipolarLine::Recalc(unsigned int width, unsigned int height,
                     HomgPoint2D& epipole, enum HomgPoint2D::EPosition epos)
{
  return Recalc(0.0, 0.0, double(width-1), double(height-1), epipole,  epos);
}

int EpipolarLine::Recalc(const double& minx,
                         const double& miny,
                         const double& maxx,
                         const double& maxy,
                         HomgPoint2D& epipole,
                         enum HomgPoint2D::EPosition epos) {
  minx_ = minx;
  miny_ = miny;
  maxx_ = maxx;
  maxy_ = maxy;

  Epipole_ = epipole;
  EpipolePosition_ = epos;
  return Recalc();
}

int
EpipolarLine::Recalc()
{
  Homogenize();
  // argument point is only used as dummy, to
  // verify if (argument-)point sits on image border
  HomgPoint2D point(minx_-1.0, miny_-1.0);

  if (!DetermineIntersectionsAndEpipoleDistances_(point)) return -1;
  Angle_ = atan2(FarIntersection_[1]-Epipole_[1],
         FarIntersection_[0]-Epipole_[0]);
  Type_ = Sym_.DetermineLineType(Angle_);
  Homogenize();
  DeltaR_ = (fabs(data_[0]) > fabs(data_[1]))?(fabs(data_[0]/data_[1])):
    (fabs(data_[1]/data_[0]));
  return 0;
}


void
EpipolarLine::Init(unsigned int width, unsigned int height,
                   HomgPoint2D& epipole, HomgPoint2D& otherpoint,
                   enum HomgPoint2D::EPosition epos)
{
  double minx = 0.0;
  double maxx = double(width-1);
  double miny = 0.0;
  double maxy = double(height-1);
  Init(minx, miny, maxx, maxy, epipole, otherpoint, epos);
}

void
EpipolarLine::Init(const double& minx, const double& miny,
                   const double& maxx, const double& maxy,
                   HomgPoint2D& epipole, HomgPoint2D& otherpoint,
                   enum HomgPoint2D::EPosition epos)
{
  PInfinity_ = otherpoint;
  PInfinityPosition_ = otherpoint.DeterminePosition(minx, miny,
                                                    maxx, maxy);
  epipole.CrossProduct(otherpoint, *this);
  Homogenize();
  Epipole_ = epipole;

  minx_ = minx;
  miny_ = miny;
  maxx_ = maxx;
  maxy_ = maxy;

  Homogenize();
  DeltaR_ = (fabs(data_[0]) > fabs(data_[1]))?(fabs(data_[0]/data_[1])):
    (fabs(data_[1]/data_[0]));

  DetermineIntersectionsAndEpipoleDistances_(otherpoint);
  Angle_ = atan2(FarIntersection_[1]-Epipole_[1],
         FarIntersection_[0]-Epipole_[0]);
  Type_ = Sym_.DetermineLineType(Angle_);
  EpipolePosition_ = epos;
}



void
EpipolarLine::Init(unsigned int width, unsigned int height,
                   HomgPoint2D& epipole, HomgPoint2D& pinf)
{
  Init(width, height, epipole, pinf,
       epipole.DeterminePosition(width, height));
}


//  template <class PixelType> double
//  EpipolarLine::ScanLine(Image<PixelType>& Source, HomgPoint2D& epipole,
//                 double ROffset, unsigned int& DestLength,
//                 PixelType *Destination, bool forward)
double
EpipolarLine::ScanLine(Image<unsigned char>& Source, HomgPoint2D& epipole,
                       enum HomgPoint2D::EPosition epos, double RMinImage,
                       double RMaxImage, bool forward,
                       unsigned int& DestLength, unsigned char *Destination)
{
  DestLength = 0;
  Homogenize();
  Epipole_ = epipole;
  minx_ = 0;
  maxx_ = Source.GetWidth()-1;
  miny_ = 0;
  maxy_ = Source.GetHeight()-1;

  Homogenize();
  DeltaR_ = (fabs(data_[0]) > fabs(data_[1]))?(fabs(data_[0]/data_[1])):
    (fabs(data_[1]/data_[0]));
  if (!DetermineIntersectionsAndEpipoleDistances_(epipole))
    return -1.0;
  Angle_ = atan2(FarIntersection_[1]-Epipole_[1],
         FarIntersection_[0]-Epipole_[0]);
  EpipolePosition_ = epos;

  Type_ = Sym_.DetermineLineType(Angle_);

  CalcTransformed_();

  BIASDOUT(D_EL_TRANSF,"TransformedLine ("<<TransformedStart_[0]<<", "
      <<TransformedStart_[1]<<")  ->  ("<<TransformedEnd_[0]<<", "
      <<TransformedEnd_[1]<<")");

  ROffset_ = (unsigned int)ceil(RMinLine_) - (unsigned int)ceil(RMinImage);

  BIASDOUT(D_EL_OFFSET,"RMinLine_: "<< RMinLine_ <<"\tRMaxLine_: "<< RMaxLine_
          <<"\tRMinImage:"<<RMinImage<<"\tRMaxImage:"<<RMaxImage
          <<"\tROffset_: "<< ROffset_);

  return ScanLine_(Source, ROffset_, DestLength, Destination, forward);
}


// input : Source, ROffset
// output : DestLength, Destination
// assumes that EpipolarLine is fully initialized
//  template <class PixelType> double
//  EpipolarLine::ScanLine_(Image<PixelType>& Source, double ROffset,
//              unsigned int& DestLength, PixelType *Destination,
//              bool forward)
double
EpipolarLine::ScanLine_(Image<unsigned char>& Source, unsigned int ROffset,
                        unsigned int& DestLength, unsigned char *Destination,
                        bool forward)
{
  unsigned int counter= 0;
  int counterincr = 0;
  unsigned int DestImageWidth =
    (unsigned int)floor(sqrt(pow(Source.GetWidth(),2.0) +
                             pow(Source.GetHeight(),2.0)))+ 1;

  if (Destination == NULL){
    Destination = new unsigned char[DestImageWidth];
    ROffset = 0;
  }

  DestLength = ROffset;

  if(forward){
    counter = DestLength;
    counterincr = 1;
  } else {
    counter = DestImageWidth - DestLength;
    counterincr = -1;
  }
  while ((TransformedCurrent_[0]<=TransformedEnd_[0])&&
         (TransformedCurrent_[1]<=TransformedEnd_[1])){
    // bilinear interpolation
    //Current_=InverseTransform_(TransformedCurrent_);
    Sym_.InverseTransform(Type_, TransformedCurrent_, Current_);
    BIASDOUT(D_EL_ROUND,"x: "<<Current_[0]<<" y: "<<Current_[1]
            <<"\tdest: "<<(int)Destination[DestLength]);
    Destination[counter] = (unsigned char)
      Source.BilinearInterpolationGrey(Current_[0], Current_[1]);

    TransformedCurrent_[0]+=dx_;
    TransformedCurrent_[1]+=dy_;
    DestLength++;
    counter += counterincr;
  }
  BIASDOUT(D_EL_OFFSET,"DestLength: " << DestLength);
  CalcTransformed_();
  Length_ = DestLength - ROffset_;
  return ROffset - RMinLine_;
}

void
EpipolarLine::Interpolate_(enum TLineType& type, unsigned char **ida, int& x,
                           int& y, double& g, unsigned char& result)
{
  result = 0;
  double gm = 1.0 - g;
  switch(type){
  case SE_E:
    BIASDOUT(D_EL_BRESROUND,x << ", " << y << " : "<< gm << "\t"
        << x << ", " << y+1 << " : "<<g);
    result = (unsigned char)((double)ida[y][x]*gm +
                 (double)ida[y+1][x]*g);
    break;
  case S_SE:
    BIASDOUT(D_EL_BRESROUND,y << ", " << y << " : "<< gm << "\t"
        << y << ", " << x+1 << " : "<<g);
    result = (unsigned char)((double)ida[x][y]*gm +
                 (double)ida[x+1][y]*g);
    //        Original[0] = Transformed[1];
    //        Original[1] = Transformed[0];
    break;
  case SW_S:
    BIASDOUT(D_EL_BRESROUND,y << ", " << -x << " : "<< gm << "\t"
        << y << ", " << -x+1 << " : "<<g);
    result = (unsigned char)((double)ida[x][-y]*gm +
                 (double)ida[x+1][-y]*g);
    //        Original[0] = Transformed[1];
    //        Original[1] = -Transformed[0];
    break;
  case W_SW:
    BIASDOUT(D_EL_BRESROUND,-x << ", " << y << " : "<< gm << "\t"
        << -x << ", " << y+1 << " : "<<g);
    result = (unsigned char)((double)ida[y][-x]*gm +
                 (double)ida[y+1][-x]*g);
    //        Original[0] = -Transformed[0];
    //        Original[1] = Transformed[1];
    break;
  case NW_W:
    BIASDOUT(D_EL_BRESROUND,-x << ", " << -y << " : "<< gm << "  : "
        <<(int)ida[-y][-x]<<"\t" << -x << ", " << -y-1 << " : "<<g
        <<"  : "<<(int)ida[-y-1][-x]);
    result = (unsigned char)((double)ida[-y][-x]*gm +
                 (double)ida[-y-1][-x]*g);
    BIASDOUT(D_EL_BRESROUND,"result: "<<(int)result);
    //        Original = Transformed * (-1);
    break;
  case N_NW:
    BIASDOUT(D_EL_BRESROUND,-y << ", " << -x << " : "<< gm << "\t"
        << -y << ", " << -x+1 << " : "<<g);
    result = (unsigned char)((double)ida[-x][-y]*gm +
                 (double)ida[-x-1][-y]*g);
    //        Original[0] = -Transformed[1];
    //        Original[1] = -Transformed[0];
    break;
  case NE_N:
    BIASDOUT(D_EL_BRESROUND,-y << ", " << x << " : "<< gm << "\t"
        << -y << ", " << x+1 << " : "<<g);
    result = (unsigned char)((double)ida[-x][y]*gm +
                 (double)ida[-x-1][y]*g);
    //        Original[0] = -Transformed[1];
    //        Original[1] = Transformed[0];
    break;
  case E_NE:
    BIASDOUT(D_EL_BRESROUND,x << ", " << -y << " : "<< gm << "\t"
        << x << ", " << -y+1 << " : "<<g);
    result = (unsigned char)((double)ida[-y][x]*gm +
                 (double)ida[-y-1][x]*g);
    //cerr << " y " << -y << "  g " << gm << endl;
    //        Original[0] = Transformed[0];
    //        Original[1] = -Transformed[1];
    break;
  case LT_Invalid:
    BIASERR("Invalid LineType, maybe wrong slope in EpipolarLine");
    break;
  }
}

void EpipolarLine::CalcTransformed_()
{
  double dr=0.0;
  Vector2<double> Transfslope(0.0, 0.0), Slope(data_[1], data_[0]);
//   dx_ = fabs(data_[0]);
//   dy_ = fabs(data_[1]);
  Sym_.Transform(Type_, Slope, Transfslope);
  dx_ = fabs(Transfslope[0]);
  dy_ = fabs(Transfslope[1]);

  BIASDOUT(D_EL_TRANSF," dx_ "<<dx_<<"\tdy_ "<<dy_<<"\ttype: "<<Type_);

  Sym_.Transform(Type_, CloseIntersection_, TransformedStart_);
  Sym_.Transform(Type_, FarIntersection_, TransformedEnd_);

  dr = ceil(RMinLine_) - RMinLine_;
  BIASDOUT(D_EL_TRANSF," dr start: "<<dr<<"\tstart befor "
          <<(Vector3<double>)TransformedStart_);
  TransformedStart_[0] += dx_ * dr;
  TransformedStart_[1] += dy_ * dr;

  Sym_.InverseTransform(Type_, TransformedStart_, Start_);

  dr = RMaxLine_ - floor(RMaxLine_);
  BIASDOUT(D_EL_TRANSF," dr end: "<<dr<<"\tend befor "
          <<(Vector3<double>)TransformedEnd_);
  TransformedEnd_[0] -= dx_ * dr;
  TransformedEnd_[1] -= dy_ * dr;

  Sym_.InverseTransform(Type_, TransformedEnd_, End_);

  BIASDOUT(D_EL_TRANSF,"("<<CloseIntersection_[0]<<", "<<CloseIntersection_[1]
      <<") -> ("<<FarIntersection_[0]<<", "<<FarIntersection_[1]
      <<")  transf:  ("<<TransformedStart_[0]<<", "<<TransformedStart_[1]
      <<") -> ("<<TransformedEnd_[0]<<", "<<TransformedEnd_[1]<<")");

  TransformedCurrent_ = TransformedStart_;
  Sym_.InverseTransform(Type_, TransformedCurrent_, Current_);

}

// the following is used in DetermineIntersectionsAndEpipoleDistances_
int CompIntersect(const void *left, const void *right)
{
  struct TIntersect *Left = (struct TIntersect *)left;
  struct TIntersect *Right = (struct TIntersect *)right;
  return (Left->EpipoleDistance < Right->EpipoleDistance);
}

bool
EpipolarLine::DetermineIntersectionsAndEpipoleDistances_(HomgPoint2D& point)
{
  bool result = true;
  int intersectioncount = 0;
  struct TIntersect edges[4]; // 0=left, 1=right, 2=top, 3=bottom
  HomgLine2D left(-1.0, 0.0, minx_);
  HomgLine2D right(-1.0, 0.0, maxx_);
  HomgLine2D top(0.0, -1.0, miny_);
  HomgLine2D bottom(0.0, -1.0, maxy_);

  edges[0].IntersectsInImage = edges[1].IntersectsInImage
    = edges[2].IntersectsInImage = edges[3].IntersectsInImage = false;
  edges[0].EpipoleDistance = edges[1].EpipoleDistance
    = edges[2].EpipoleDistance = edges[3].EpipoleDistance = -1.0;
  /// the lines describing the image borders
  edges[0].Edge = LeftEdge;
  edges[1].Edge = RightEdge;
  edges[2].Edge = TopEdge;
  edges[3].Edge = BottomEdge;
  /// compute the intersection of the image borders with the epipolar lines
  edges[0].Point = Intersection(left);
  if (!edges[0].Point.IsAtInfinity()) edges[0].Point.Homogenize();
  else edges[0].Point.Set(DBL_MAX, DBL_MAX, 0.0);
  edges[1].Point = Intersection(right);
  if (!edges[1].Point.IsAtInfinity()) edges[1].Point.Homogenize();
  else edges[1].Point.Set(DBL_MAX, DBL_MAX, 0.0);
  edges[2].Point = Intersection(top);
  if (!edges[2].Point.IsAtInfinity()) edges[2].Point.Homogenize();
  else edges[2].Point.Set(DBL_MAX, DBL_MAX, 0.0);
  edges[3].Point = Intersection(bottom);
  if (!edges[3].Point.IsAtInfinity()) edges[3].Point.Homogenize();
  else edges[3].Point.Set(DBL_MAX, DBL_MAX, 0.0);

  // deal with compiler precision
  for (unsigned int i=0; i<4; i++){
    if (edges[i].Point[0]<minx_ &&  edges[i].Point[0]>minx_-MY_DBL_PRECISION)
      edges[i].Point[0] = minx_;
    if (edges[i].Point[1]<miny_ &&  edges[i].Point[1] >miny_-MY_DBL_PRECISION)
      edges[i].Point[1] = miny_;
    if (edges[i].Point[0] > maxx_ &&
        edges[i].Point[0] < maxx_+MY_DBL_PRECISION)
      edges[i].Point[0] = maxx_;
    if (edges[i].Point[1] > maxy_ &&
        edges[i].Point[1] < maxy_+MY_DBL_PRECISION)
      edges[i].Point[1] = maxy_;
  }

  //  #ifdef BIAS_DEBUG
  //    cout << "untested intersections: " << endl;
  //    for (int i=0; i<4; i++){
  //      cout << i <<":  (" << edges[i].Point[0] << ", "
  //     << edges[i].Point[1] << ",  "<< edges[i].Point[2] <<")\t"
  //     << edges[i].EpipoleDistance << endl;
  //    }
  //  #endif

  // check which intersection lie actually in the image
  if ((point[0]==minx_) ||
      ((edges[0].Point[2]!=0.0) &&
       (edges[0].Point[1]>=miny_ && edges[0].Point[1]<=maxy_))){
    if(point[0]==minx_) edges[0].Point=point;
    edges[0].IntersectsInImage = true; intersectioncount++;
    edges[0].EpipoleDistance = sqrt(pow(edges[0].Point[1]-Epipole_[1],2) +
                                    pow(edges[0].Point[0]-Epipole_[0],2));
  }
  if ((point[0]==maxx_) ||
      ((edges[1].Point[2]!=0.0) &&
       (edges[1].Point[1]>=miny_ && edges[1].Point[1]<=maxy_))){
    if(point[0]==maxx_) edges[1].Point=point;
    edges[1].IntersectsInImage = true; intersectioncount++;
    edges[1].EpipoleDistance = sqrt(pow(edges[1].Point[1]-Epipole_[1],2) +
                                    pow(edges[1].Point[0]-Epipole_[0],2));
  }
  if ((point[1]==miny_) ||
      ((edges[2].Point[2]!=0.0) &&
       (edges[2].Point[0]>=minx_ && edges[2].Point[0]<=maxx_))){
    if(point[1]==miny_) edges[2].Point=point;
    edges[2].IntersectsInImage = true; intersectioncount++;
    edges[2].EpipoleDistance = sqrt(pow(edges[2].Point[1]-Epipole_[1],2) +
                                    pow(edges[2].Point[0]-Epipole_[0],2));
  }
  if ((point[1]==maxy_) ||
      ((edges[3].Point[2]!=0.0) &&
       (edges[3].Point[0]>=minx_ && edges[3].Point[0]<=maxx_))){
    if(point[1]==maxy_) edges[3].Point=point;
    edges[3].IntersectsInImage = true;intersectioncount++;
    edges[3].EpipoleDistance = sqrt(pow(edges[3].Point[1]-Epipole_[1],2) +
                                    pow(edges[3].Point[0]-Epipole_[0],2));
  }

 //  #ifdef BIAS_DEBUG
//      cout << "unsorted intersections: " << endl;
//      for (int i=0; i<4; i++){
//        cout << i<<" intersects:"<< edges[i].IntersectsInImage <<":  (" << edges[i].Point[0] << ", "
//       << edges[i].Point[1] << ",  "<< edges[i].Point[2] <<")\t"
//       << edges[i].EpipoleDistance << endl;
//      }
//    #endif

  // sort Intersections according to epipole distance
  qsort(&edges, 4, sizeof(struct TIntersect), CompIntersect);

 //   #ifdef BIAS_DEBUG
//      cout << "sorted intersections: " << endl;
//      for (int i=0; i<4; i++){
//        cout << i <<" intersects:"<< edges[i].IntersectsInImage<< " at:  (" << edges[i].Point[0] << ", "
//       << edges[i].Point[1] << ")\t" << edges[i].Point[2] << ")\t"<< edges[i].EpipoleDistance << endl;
//      }
//    #endif

//   if (EpipolePosition_!= Center){

    FarIntersection_ = edges[0].Point;
    FarEdge_ = edges[0].Edge;
    RMaxLine_ = edges[0].EpipoleDistance;
    switch(intersectioncount){
    case 1:
      CloseIntersection_=edges[0].Point;
      CloseEdge_ = edges[0].Edge;
      RMinLine_ = edges[0].EpipoleDistance;
      break;
    case 2:
      CloseIntersection_=edges[1].Point;
      CloseEdge_ = edges[1].Edge;
      RMinLine_ = edges[1].EpipoleDistance;
      break;
    case 3:
      CloseIntersection_=edges[2].Point;
      CloseEdge_ = edges[2].Edge;
      RMinLine_ = edges[2].EpipoleDistance;
      break;
    case 4:
      CloseIntersection_=edges[3].Point;
      CloseEdge_ = edges[3].Edge;
      RMinLine_ = edges[3].EpipoleDistance;
      break;
    default:
      result = false;
#ifdef BIAS_DEBUG
      BIASDOUT(D_EL_INT, "invalid intersection count "<<intersectioncount
               <<" of ep. line: "<<*this<<"  with epipole: "<<Epipole_
               <<"close intersect: "<<GetCloseIntersectionWithImageBorder()
               <<"far intersection: "<<GetFarIntersectionWithImageBorder()
               <<"Image borders are minx, miny, maxx, maxy="
               <<minx_<<" "<< miny_<<" "<< maxx_<<" "<< maxy_
               << " sorted intersections: ");
      for (int i=0; i<4; i++){
        BIASDOUT(D_EL_INT, i <<":  (" << edges[i].Point[0] << ", "
                 << edges[i].Point[1] << ")\t" << edges[i].EpipoleDistance);
      }
      //BIASASSERT(intersectioncount>=2);
#endif
      break;
    }
  return result;
}



#include <limits> // jw
template <class PixelType>
void EpipolarLine::Draw(Image<PixelType>& im)
{
  unsigned int start[2], end[2];
  PixelType color[3];
  color[0]=PixelType(255);
  color[1]=PixelType(255);
  color[2]=PixelType(255);

//   cerr << "Epipole: "<<Epipole_<<"   PInfinity: "<<PInfinity_<<endl;
//   cerr << "EpipolePos: "<<EpipolePosition_<<"    pinfpos: "<<PInfinityPosition_<<endl;
//   cerr << GetCloseIntersectionWithImageBorder() << " \t\t"
//        << GetFarIntersectionWithImageBorder() << endl;
  if (EpipolePosition_!=HomgPoint2D::Center){
    start[0]=(unsigned int)rint(GetCloseIntersectionWithImageBorder()[0]/
                                GetCloseIntersectionWithImageBorder()[2]);
    start[1]=(unsigned int)rint(GetCloseIntersectionWithImageBorder()[1]/
                                GetCloseIntersectionWithImageBorder()[2]);
  } else {
    start[0]=(unsigned int)rint(Epipole_[0]/Epipole_[2]);
    start[1]=(unsigned int)rint(Epipole_[1]/Epipole_[2]);
    ImageDraw<PixelType>::RectangleCenter(im, start, 5,color);
  }

  if (PInfinityPosition_!=HomgPoint2D::Center){
    end[0]=(unsigned int)rint(GetFarIntersectionWithImageBorder()[0]/
                     GetFarIntersectionWithImageBorder()[2]);
    end[1]=(unsigned int)rint(GetFarIntersectionWithImageBorder()[1]/
                     GetFarIntersectionWithImageBorder()[2]);
  } else {
    end[0]=(unsigned int)rint(PInfinity_[0]/PInfinity_[2]);
    end[1]=(unsigned int)rint(PInfinity_[1]/PInfinity_[2]);
    ImageDraw<PixelType>::RectangleCenter(im, end, 5,color);
  }
  //cerr<<"start"<<start[0]<<" "<<start[1]<<"  end: "<<end[0]<<" "<<end[1]<<endl;
  ImageDraw<PixelType>::Line(im, start, end,color);
}

template <class PixelType>
void EpipolarLine::DrawWhole(Image<PixelType>& im)
{
  CloseIntersection_.Homogenize();
  FarIntersection_.Homogenize();

  if (!im.IsPositionInImage(int(rint( CloseIntersection_[0])),
                            int(rint( CloseIntersection_[1])))) {
    BIASERR("close intersection not in image: "<<CloseIntersection_);
    return;
  }
  if (!im.IsPositionInImage(int(rint( FarIntersection_[0])),
                            int(rint( FarIntersection_[1])))) {
    BIASERR("far intersection not in image: "<<FarIntersection_);
    return;
  }

  unsigned int start[2], end[2];
  start[0] = (unsigned int)rint( CloseIntersection_[0]);
  start[1] = (unsigned int)rint( CloseIntersection_[1]);
  end[0]   = (unsigned int)rint( FarIntersection_[0]);
  end[1]   = (unsigned int)rint( FarIntersection_[1]);
  ImageDraw<PixelType>::Line(im, start, end);
}

template <class PixelType>
void EpipolarLine::
DrawDistortedLine(const Vector3<double>& epipoleRayOrigImage,
                  const Vector3<double>& infRayOrigImage,
                  Image<PixelType>& image,
                  ProjectionParametersBase* paramsOrig,
                  PixelType* Color,
                  int lineWidth) {

  unsigned int width(0), height(0);
  paramsOrig->GetImageSize(width, height);

  // need 5 cubefaces for cameras which dont have a too large field of view
  // (<270degrees), will use only one for perspective cams
  unsigned int numbercubefaces = 6;

  ProjectionParametersPerspective* paramsPersp =
    dynamic_cast<ProjectionParametersPerspective* >(paramsOrig);
  // check if we have a perspective camera, otherwise forward-cube-map
  // epipolar lines

  // have to compute cube-map and forward-map cub-faces into image
  // with ideal KMatrix
  KMatrix K(MatrixIdentity);
  double cubefacewidth = double(width+height)/2.0;
  // make slightly more than 90 deg fov
  K[0][0] = K[1][1] = cubefacewidth / 2.1;
  K[0][2] = K[1][2] = cubefacewidth / 2.0;

  // this is only a first workaround
  // we should really force every projectionparameters to supply
  // a GetFakeKMatrix method and then just call base->GetFakeK
  // for now, this is only implmented for spherical cams
  ProjectionParametersSpherical *pSphere =
    dynamic_cast<ProjectionParametersSpherical* >(paramsOrig);

  //double minZLocal = 0.01;
  if (paramsPersp!=NULL) {
    // need only front face, much faster !
    numbercubefaces = 1;
    if (double(width)>cubefacewidth) cubefacewidth = width;
    if (double(height)>cubefacewidth) cubefacewidth = height;
    K = paramsPersp->GetK();
    if (paramsPersp->IsDistorted()) {
      BIASWARN("distortion resolution is heuristic, may get aliased epiline !");
      // make image larger !!!
      K[0][2] += cubefacewidth;
      K[1][2] += cubefacewidth;
      cubefacewidth *= 3.0;
    }
    //minZLocal =  paramsPersp->GetMinZLocal();
  } else if (pSphere!=NULL) {
    K = pSphere->GetFakeKMatrix(cubefacewidth, 1, 45.1 * M_PI / 180.0);
  }
  paramsPersp = new ProjectionParametersPerspective;
  paramsPersp->SetK(K);
  paramsPersp->SetImageSize((unsigned int)(cubefacewidth),
                            (unsigned int)(cubefacewidth));
  paramsPersp->SetMinZLocal(0/*minZLocal*/);

  for (unsigned int cubeface = 0; cubeface<numbercubefaces; cubeface++) {
    // make cube face rotations +-90 degrees
    Vector3<double> axis(1,0,0);
    double angle = 0.0;
    switch (cubeface) {
    case 1:axis[0]=1.0; axis[1]=0.0; axis[2]=0.0; angle=M_PI/2.0; break;
    case 2:axis[0]=1.0; axis[1]=0.0; axis[2]=0.0; angle=-M_PI/2.0; break;
    case 3:axis[0]=0.0; axis[1]=1.0; axis[2]=0.0; angle=M_PI/2.0; break;
    case 4:axis[0]=0.0; axis[1]=1.0; axis[2]=0.0; angle=-M_PI/2.0; break;
    case 5:axis[0]=0.0; axis[1]=1.0; axis[2]=0.0; angle=M_PI; break;
    }
    RMatrix RCube(axis,angle);
    RMatrix RCubeTransposed = RCube.Transpose();
    paramsPersp->SetR(paramsOrig->GetR()*RCube);
    paramsPersp->SetC(paramsOrig->GetC());

    // we have the projection of the cube face, compute geometry of epiline:
    Vector3<double> epipoleRay = RCubeTransposed * epipoleRayOrigImage;
    Vector3<double> infRay     = RCubeTransposed * infRayOrigImage;

    HomgPoint2D epipole = paramsPersp->GetK()*epipoleRay;
    epipole.Homogenize();
    //HomgPoint2D epipole = paramsPersp->ProjectLocal(epipoleRay);
    //if (epipole.NormL2()==0.0) {
      // epipole is behind camera, compute anyway
    //epipole =  paramsPersp->ProjectLocal(-1.0 * epipoleRay);
    //}

    HomgPoint2D pinf = paramsPersp->ProjectLocal(infRay);
    if (pinf.NormL2()==0.0) {
      pinf = paramsPersp->ProjectLocal(-1.0 * infRay);
    }


    EpipolarLine line(epipole.CrossProduct(pinf));
    if (line.NormL2()<1e-10) {
      BIASERR("Epipolar line is strange: "<<line<<" with epipole ray "
              <<epipoleRay<<" ("<<epipole <<") and infray "<< infRay <<"("<<pinf<<")"
              <<" at current cube side "
              << *paramsPersp);
      return;
    }
    line.Normalize();

    // get relation to image
    HomgPoint2D::EPosition epos =
      epipole.DeterminePosition(int(rint(cubefacewidth)),
                                int(rint(cubefacewidth)));
    // is line in image ?
    if (line.Recalc(int(rint(cubefacewidth)), int(rint(cubefacewidth)),
                    epipole, epos)!=0) {
      BIASWARN("Epipolar line is not in image");
      continue;
    }
    // get start and end point of line in image
    int start[2], end[2];
    start[0] =
      (int)rint(line.GetCloseIntersectionWithImageBorder()[0]);
    start[1] =
      (int)rint(line.GetCloseIntersectionWithImageBorder()[1]);
    end[0]   =
      (int)rint(line.GetFarIntersectionWithImageBorder()[0]);
    end[1]   =
      (int)rint(line.GetFarIntersectionWithImageBorder()[1]);


    // paint the pixels using forward mapping
    int next[2]={start[0],start[1]};
    Bresenham bres(start, end);
    bool lastPixelValid = false;
    int lastPixel[2];

    PixelType *color = new PixelType [image.GetChannelCount()], *mycolor=NULL;
    for (unsigned int c=0; c<image.GetChannelCount(); c++)
      color[c] = PixelType(255);

    // if no color, draw in white
    if (Color==NULL)
      mycolor = color;
    else mycolor = Color;

    double linefac = sqrt(line[0]* line[0]+ line[1]*line[1]);
    if (linefac<1e-8) linefac = 1;
    else linefac = 1.0/linefac;
    while (bres.GetNext(next)) {
      // next grid pixel on cube face
      HomgPoint2D point(next[0], next[1], 1.0);
      // compute distance from mathematical epipolar line (discretized grid)
      double dist = linefac * point.ScalarProduct(line);
      // project point onto line, get subpixel coordinates
      point[0] -= dist * linefac * line[0];
      point[1] -= dist * linefac * line[1];

      // compute ray in space
      Vector3<double> pos, ray;
      paramsPersp->UnProjectToRay(point, pos, ray);
      // check if this ray is visible in the orig image
      if (paramsOrig->DoesPointProjectIntoImage(HomgPoint3D(paramsOrig->GetC()
                                                            + ray), point)) {
        // it is within the calibrated area
        int thisPixel[2];
        thisPixel[0] = (int)(rint(point[0]));
        thisPixel[1] = (int)(rint(point[1]));
        // is it also within the image ?
        if (image.IsPositionInImage(thisPixel[0],thisPixel[1])) {
          // make just a point or connect from last point (nicer line)
          if (lastPixelValid) {
            // connect neigboring pixels
            ImageDraw<PixelType>::Line(image, lastPixel, thisPixel,  mycolor, lineWidth);
          } else {
            // last one was invalid, just mark a pixel
            for (unsigned int c=0; c<image.GetChannelCount(); c++) {
              image.SetPixel(mycolor[c], thisPixel[0], thisPixel[1], c);
            }
          }
          // remember this pixel for next line
          lastPixel[0] = thisPixel[0];
          lastPixel[1] = thisPixel[1];
          lastPixelValid = true;
        } else {
          // cannot draw this one ...
          lastPixelValid = false;
        }
      }
      else {
        // it is not within the viewing frustum
        lastPixelValid = false;
      }
    } // end while
    delete [] color;
  }
  delete paramsPersp;
  paramsPersp = NULL;
}


template <class PixelType>
void EpipolarLine::ProjectEpipolarPlane(const Projection& thisP,
                                        Image<PixelType> &thisIm,
                                        const Vector3<double>& otherCamCenter,
                                        const Vector3<double>& RayDir,
                                        PixelType* Color, 
                                        int lineWidth) {
  ProjectionParametersBase* paramsOrig =
    const_cast<ProjectionParametersBase*>(thisP.GetParameters());

  HomgPoint3D PointAtInfinity(RayDir);
  PointAtInfinity[3] = 0.0;
  cout<<"EpipolarLine::Point at infinity is "<<PointAtInfinity<<endl;

  // get end of epipolar line
  HomgPoint2D infPointOrigImage(0,0,0);
  if (thisP.DoesPointProjectIntoImage(PointAtInfinity,
                                      infPointOrigImage)) {
    infPointOrigImage.Homogenize();
    // show point transferred with H_inf
    ImageDraw<PixelType>::RectangleCenter(thisIm,
                                          int(rint(infPointOrigImage[0])),
                                          int(rint(infPointOrigImage[1])),
                                          2);
  } else {
    cout<<"EpipolarLine::Point at infinity does not project into image 2:"<<endl;
  }
  HomgPoint2D infRayOrigImage = paramsOrig->GetExternals()*PointAtInfinity;
  if (infRayOrigImage.NormL2()<1e-10) {
    BIASERR("invalid point"<<PointAtInfinity);
    return;
  }
  infRayOrigImage.Normalize();

  // check if we really have camera translation
  bool onlyHomography = (otherCamCenter-thisP.GetC()).NormL2()<1e-5;


  // get start of epipolar line
  if (!onlyHomography){
    // compute epipoles in big image
    HomgPoint2D epipoleOrigImage(0,0,0);
    if (thisP.DoesPointProjectIntoImage(HomgPoint3D(otherCamCenter),
                                        epipoleOrigImage)) {
      // draw "bold" circle at epipole
      PixelType Value[3];
      Value[0] = PixelType(255);
      Value[1] = PixelType(255);
      Value[2] = PixelType(255);
      ImageDraw<PixelType>::CircleCenter(thisIm,
                                         int(rint(epipoleOrigImage[0])),
                                         int(rint(epipoleOrigImage[1])),
                                         5, Value);
      ImageDraw<PixelType>::CircleCenter(thisIm,
                                         int(rint(epipoleOrigImage[0])),
                                         int(rint(epipoleOrigImage[1])),
                                         6, Value);
    }
    HomgPoint2D epipoleRayOrigImage =
      paramsOrig->GetExternals() * HomgPoint3D(otherCamCenter);
    // must be possible, because not same camera center :
    epipoleRayOrigImage.Normalize();

    // compute the projection of the epipolar plane for that point
    EpipolarLine eline = epipoleRayOrigImage.CrossProduct(infRayOrigImage);
    if (eline.NormL2()>1e-10) {
      cout<<"Drawing distorted line"<<endl;
      DrawDistortedLine(epipoleRayOrigImage, infRayOrigImage,
                        thisIm, paramsOrig, Color, lineWidth);
    } else {
      cout<<"epipole dir and ray dir coincident !"<<endl;
    }
  } else {
    BIASERR("pure homography. no epiline.");
  }

}

#define INSTANCE(type) \
template void BIASGeometry_EXPORT EpipolarLine::Draw<type>(Image<type>& im);\
template void BIASGeometry_EXPORT EpipolarLine::DrawWhole<type>(Image<type>& im); \
template void BIASGeometry_EXPORT EpipolarLine::ProjectEpipolarPlane<type>(const Projection& \
                                                       thisP, \
                                                       Image<type> &thisIm, \
                                                       const Vector3<double>& \
                                                       otherCamCenter, \
                                                       const Vector3<double>& \
                                                       RayDir, type* color, int lineWidth); \
 template void BIASGeometry_EXPORT EpipolarLine::DrawDistortedLine<type>(const Vector3<double>& epipoleRayOrigImage, const Vector3<double>& infRayOrigImage, Image<type>& image, ProjectionParametersBase* paramsOrig, type* color, int lineWidth);

INSTANCE(unsigned char)
INSTANCE(float)

#ifdef BUILD_IMAGE_CHAR
INSTANCE(char)
#endif
#ifdef BUILD_IMAGE_DOUBLE
INSTANCE(double)
#endif
#ifdef BUILD_IMAGE_UINT
INSTANCE(unsigned int)
#endif
#ifdef BUILD_IMAGE_INT
INSTANCE(int)
#endif
#ifdef BUILD_IMAGE_SHORT
INSTANCE(short)
#endif
#ifdef BUILD_IMAGE_USHORT
INSTANCE(unsigned short)
#endif