Conic2D.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 "Conic2D.hh"
#include <Base/Geometry/HomgLine2D.hh>
#include <float.h>
#include <MathAlgo/PolynomialSolve.hh>
#include <MathAlgo/SVD.hh>
#include <Base/Math/Matrix2x2.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Base/Image/Image.hh>
#include <Geometry/Quadric3D.hh>
#include <Geometry/PMatrix.hh>
#include <Geometry/FMatrixEstimation.hh>

#define SVD_ZERO_THRESH DBL_EPSILON * 16.0

using namespace BIAS;
using namespace std;

double Conic2D::GetDiscriminant() const
{
  return ((*this)[0][0]*(*this)[1][1]-(*this)[0][1]*(*this)[0][1]);
}

bool Conic2D::IsDoubleLine() const {
  // degenerate case ?
  if (fabs(GetDeterminant())>DBL_EPSILON) {
    // rank < 3
    //cout << "rank<3, Det=" <<det()<<endl;
    return false;
  } 
  if (GetSignature() != 0) {
    // Degenerated case 
    //cout << "Degenerated case" <<GetSignature()<<endl;
    return false;
  }
  return true;
}

bool Conic2D::IsSingleLine() const {
  // degenerate case ?
  if (fabs(GetDeterminant())>DBL_EPSILON) {
    // rank < 3
    //cout << "rank<3, Det=" <<det()<<endl;
    return false;
  } 
  if (GetSignature() != 1) {
    // Degenerated case 
    //cout << "Degenerated case" <<GetSignature()<<endl;
    return false;
  }
  return true;
}

bool Conic2D::IsPoint() const {
  // degenerate case ?
  if (fabs(GetDeterminant())>DBL_EPSILON) {
    // rank < 3
    //cout << "rank<3, Det=" <<det()<<endl;
    return false;
  } 
  if (GetSignature() != 2) {
    // Degenerated case 
    //cout << "Degenerated case" <<GetSignature()<<endl;
    return false;
  }
  return true;
}

bool Conic2D::IsEllipse() const {
  // degenerate case ?
  if (fabs(GetDeterminant())<=DBL_EPSILON) {
    // rank < 3
    //cout << "rank<3, Det=" <<det()<<endl;
    return false;
  }

  if (GetSignature() != 1) {
    // Degenerated case 
    //cout << "Degenerated case" <<GetSignature()<<endl;
    return false;
  }

  // parabola or hyperbola ?
  if (GetDiscriminant()<-DBL_EPSILON) {
    //cerr <<"GetDiscriminant"<<endl;
    return false;
  }
  if ((*this)[0][0]+(*this)[1][1]>0.0) {
    // ok, should be ellipse
    return true;
  }
  // must be empty set
  //cerr <<(*this)[0][0] << "/" << (*this)[1][1]<<endl;
  return false;
}

bool Conic2D::IsParabola() const {
  // degenerate case ?
  if (fabs(GetDeterminant())<=DBL_EPSILON) {
    // rank < 3
    //cout << "rank<3, Det=" <<det()<<endl;
    return false;
  }

  if (GetSignature() != 1) {
    // Degenerated case 
    //cout << "Degenerated case" <<GetSignature()<<endl;
    return false;
  }

  if (fabs(GetDiscriminant())<DBL_EPSILON) {
    //cerr <<"GetDiscriminant"<<endl;
    return true;
  }
  return false;
}

bool Conic2D::IsHyperbola() const {
  // degenerate case ?
  if (fabs(GetDeterminant())<=DBL_EPSILON) {
    // rank < 3
    //cout << "rank<3, Det=" <<det()<<endl;
    return false;
  }

  if (GetSignature() != 1) {
    // Degenerated case 
    //cout << "Degenerated case" <<GetSignature()<<endl;
    return false;
  }

  // parabola or hyperbola ?
  if (GetDiscriminant()>=-DBL_EPSILON) {
    //cerr <<"GetDiscriminant"<<endl;
    return false;
  }
  return true;
}

bool Conic2D::IsEmpty() const {
  // degenerate case ?
  if (fabs(GetDeterminant())<=DBL_EPSILON) {
    // rank < 3
    //cout << "rank<3, Det=" <<det()<<endl;
    return false;
  } 
  if (GetSignature() != 3) {
    // Degenerated case 
    //cout << "Degenerated case" <<GetSignature()<<endl;
    return false;
  }
  return true;
}

bool Conic2D::IsProper() const {
  double norm = this->NormFrobenius();
  if (fabs(norm)<1e-9) return false;
  //cout<<"det is "<<det()<<" norm is "<<norm<<endl;
  return (fabs(GetDeterminant() / (norm*norm*norm))>1e-9);
}

Conic2D::ConicType Conic2D::GetConicType() const {
  const unsigned int Signature = GetSignature();
  if (!IsProper()) {
    // degenerate case: cone center is on conic
    switch (Signature) {
    case 1: return Conic2D::SingleLine;
    case 2: return Conic2D::Point;
    }
    // must be signature 0
    if (NormL2()>DBL_EPSILON) return Conic2D::DoubleLine;
    // we have a zero matrix
    return Conic2D::WholePlane;
  } else {
    // cone center is not on conic
    if (Signature==3) return Conic2D::Empty; // no real points
    const double disc = GetDiscriminant();
    if (disc>DBL_EPSILON) return Conic2D::Ellipse;
    if (disc<-DBL_EPSILON) return Conic2D::Hyperbola;
    return Conic2D::Parabola;
  }
  // can never happen but wont produce "no return value" warning 
  // no, unreachable! (jw)
  //return Conic2D::Unknown;
}

int Conic2D::GetPoint(HomgPoint2D& thepoint) const {
  BIASASSERT(IsPoint());
  SVD mysvd((Matrix<CONIC2D_TYPE>)*this);
  thepoint = mysvd.GetNullvector();
  thepoint.Normalize();
  if (fabs(thepoint[2])>1e-10) thepoint.Homogenize();
  return 0;
}


void Conic2D::Draw(Image<unsigned char>& img) const {
#ifdef BIAS_DEBUG
  if (IsEllipse()) {
    BIASERR("Better use ImageDraw::Ellipse for Conic drawing.");
  }
#endif

  const unsigned int uiHeight = img.GetHeight();
  const unsigned int uiWidth  = img.GetWidth();
  register unsigned char** ppI = img.GetImageDataArray();

  HomgPoint2D p(0,0);
  p.Homogenize();
  register int LastSign = 0;
  double dist;
  // dumb drawing algorithm (improve this using parameterized drawing)
  // run over all pixels and check where the sign changes
  // wont catch all ellipses with some radius<1
  for (register unsigned int y=0;y<uiHeight; y++) {
    p[0] = -1; p[1] = y;
    dist = LocatePoint(p);
    if (dist>0.0) LastSign = 1;
    else if (dist<0.0) LastSign = -1;
    else LastSign = 0 ;
    for (register unsigned int x=0; x<uiWidth; x++) {
      p[0] = x; p[1] = y;
      dist = LocatePoint(p);
      if (dist<0.0) {
        if (LastSign>0) { 
          ppI[y][x] = (ppI[y][x]>127)?0:255;
          cout << "D";
        }
        LastSign = -1;
      } else if (dist>0.0) {
        if (LastSign<0) {
          ppI[y][x] = (ppI[y][x]>127)?0:255;
          cout << "D";
        }
        LastSign = 1;
      } else {
        LastSign = 0;
        ppI[y][x] = (ppI[y][x]>127)?0:255;
        cout << "D";
      }
    }
  }
  // run into perpendicular direction to catch x-parallel lines
  for (register unsigned int x=0;x<uiWidth; x++) {
    p[0] = x; p[1] = -1;
    dist = LocatePoint(p);
    if (dist>0.0) LastSign = 1;
    else if (dist<0.0) LastSign = -1;
    else LastSign = 0 ;
    for (register unsigned int y=0; y<uiHeight; y++) {
      p[0] = x; p[1] = y;
      dist = LocatePoint(p);
      // cout << dist <<" ";
      if (dist<0.0) {
        if (LastSign>0) { 
          ppI[y][x] = (ppI[y][x]>127)?0:255;
           cout << "D";
        }
        LastSign = -1;
      } else if (dist>0.0) {
        if (LastSign<0) {
          ppI[y][x] = (ppI[y][x]>127)?0:255;
          cout << "D";
        }
        LastSign = 1;
      } else {
        LastSign = 0;
        ppI[y][x] = (ppI[y][x]>127)?0:255;
        cout << "D";
      }
    }
  }
}

void Conic2D::SetEllipse(const HomgPoint2D& Center, const double& dAngle,
                         const double& radius_a, const double& radius_b) {
  
  SetZero();
  (*this)[0][0] = 1.0 / (radius_a*radius_a);
  (*this)[1][1] = 1.0 / (radius_b*radius_b);
  (*this)[2][2] = -1;
  
  Matrix3x3<double> Transform, Rotation;

  // transfer ellipse center into origin
  Transform.SetIdentity();
  Transform[0][2] = -Center[0];
  Transform[1][2] = -Center[1];
  Transform[2][2] =  Center[2];

  // rotate by dAngle around origin
  Rotation.SetIdentity();
  Rotation[1][1] =   Rotation[0][0] = cos(dAngle);
  Rotation[0][1] = -(Rotation[1][0] = sin(dAngle));
    
  // first transfer, than rotate
  Transform = Rotation * Transform;

  (*this) = (Matrix<CONIC2D_TYPE>)(Transform.Transpose() * (*this) * Transform);
  // Point * (*this) * Point now means:
  // (R * (T * Point)) * (this_canonic) * (R * (T * Point))
  // where this_canonic = diag(radius_a^2, radius_b^2, -1) is the unrotated
  // ellipse around the origin
}

void Conic2D::
SetPointAndCovariance(const HomgPoint2D& center,
                      const Matrix2x2<CONIC2D_TYPE>& cov,
                      const double& dScale)
{
  BIASASSERT(!center.IsAtInfinity());

  // the Euclidean image coordinates
  CONIC2D_TYPE cx=center[0]/center[2], cy=center[1]/center[2];
  
  // the upper left 2 by 2 matrix of the conic always consists of the inverse
  // covariance matrix
  Matrix2x2<CONIC2D_TYPE> icov;
  if (cov.Invert(icov)!=0){
    BIASASSERT(cov.Invert(icov)==0);
  }
  const double iscale=1.0/(dScale*dScale);
  (*this)[0][0] = icov[0][0] * iscale;
  (*this)[0][1] = icov[0][1] * iscale;
  (*this)[1][0] = icov[1][0] * iscale;
  (*this)[1][1] = icov[1][1] * iscale;
  
  // when center is at (0, 0, 1)^T, the conic c is simply given by the 
  // inverse covariance matrix: 
  // c = | cov^-1  0 |
  //     | 0      -1 |
  // under a translation t = |0 -center|
  // the conic c transforms to 
  // c' = t^T * c * t
  // resulting in
  (*this)[0][2]=(*this)[2][0]=-(cx*icov[0][0]+cy*icov[0][1]);
  (*this)[1][2]=(*this)[2][1]=-(cx*icov[1][0]+cy*icov[1][1]);

  (*this)[2][2]=-cx*(*this)[0][2]-cy*(*this)[1][2]-1.0;
}


bool Conic2D::
GetPointAndCovariance(HomgPoint2D& center,
                      Matrix2x2<CONIC2D_TYPE>& cov) const {
  // the discriminant is the determinant of the upper left 2 by 2 matrix
  const double dDisc = GetDiscriminant();
  if (fabs(dDisc) <= DBL_EPSILON) {
    // degenerate case
    return false;
  }

  const double  a = (*this)[0][0];
  const double  b = 0.5*((*this)[1][0]+(*this)[0][1]);
  const double  c = (*this)[1][1];
  const double  d = 0.5*((*this)[2][0]+(*this)[0][2]); 
  const double  e = 0.5*((*this)[2][1]+(*this)[1][2]); 

  // find extremum of quadratic function
  center[0] = (b * e - c * d) / dDisc;
  center[1] = (b * d - a * e) / dDisc;
  center[2] = 1;  

  // invert the upper left 2 by 2 matrix
  cov[0][0]=(*this)[1][1]/dDisc;
  cov[1][0]=-(*this)[1][0]/dDisc;
  cov[0][1]=-(*this)[0][1]/dDisc;
  cov[1][1]=(*this)[0][0]/dDisc;

  return true;
}



bool Conic2D::
GetEllipseParameters(double center[2], double a[2], double b[2]) const {
  HomgPoint2D c;
  Matrix2x2<CONIC2D_TYPE> cov;

  if (!GetPointAndCovariance(c, cov)){
    return false;
  }

  center[0]=c[0];
  center[1]=c[1];
      
  double la, lb;
  Vector2<double> va, vb;
  cov.EigenvalueDecomposition(la, va, lb, vb);

  cout << "va "<<va[0]<<", "<<va[1]<<"\tla :"<<la
       << "\nvb "<<vb[0]<<", "<<vb[1]<<"\tlb :"<<lb<<endl;

  la=sqrt(la);
  lb=sqrt(lb);

  a[0]=la*va[0];
  a[1]=la*va[1];

  b[0]=lb*vb[0];
  b[1]=lb*vb[1];

  cout << "center "<<center[0]<<", "<<center[1]<<"\ta: "<<a[0]<<", "<<a[1]
       <<"\tb: "<<b[0]<<", "<<b[1]<<endl;
  return true;
}

bool Conic2D::GetEllipseParameters(HomgPoint2D& Center, 
                                   double& dAngle,
                                   double& radius_a, 
                                   double& radius_b) const {
  if (!IsEllipse()) {
    //    BIASERR("Conic is no ellipse! Make sure Det=-1");
    return false;
  }

  /* ellipse conic is symmetric matrix of form:

     a b d
     b c e
     d e f

  */ 

  const double&  a = (*this)[0][0];
  const double   b = 0.5*((*this)[1][0]+(*this)[0][1]);
  const double&  c = (*this)[1][1];
  const double  d = 0.5*((*this)[2][0]+(*this)[0][2]); 
  const double  e = 0.5*((*this)[2][1]+(*this)[1][2]); 
  
  const double dDisc = GetDiscriminant();
  if (fabs(dDisc) <= DBL_EPSILON)   {
    // degenerate case
    return false;
  }

  // find extremum of quadratic function
  Center[0] = (b * e - c * d) / dDisc;
  Center[1] = (b * d - a * e) / dDisc;
  Center[2] = 1;

  const double q0 = LocatePoint(Center);

  double theta, lambda1, lambda2;
  const double p = (a + c) / 2.0;
  const double q = (a - c) / 2.0;
  const double r = sqrt(q * q + b * b);
  if ((fabs(b) <= DBL_EPSILON) && (fabs(q) <=DBL_EPSILON))
    theta = 0.0;
  else
    theta = 0.5 * atan2(b, q);
 
  dAngle = -theta;

  lambda1 = p + r;
  lambda2 = p - r;


  if ((fabs(lambda1) <= DBL_EPSILON) || 
      (fabs(lambda2) <= DBL_EPSILON) || 
      (fabs(q0) <= DBL_EPSILON)) {
    // degenerate
    return false;
  }

  lambda1 /= -q0;
  lambda2 /= -q0;

  radius_a = 1.0 / sqrt(fabs(lambda1));
  radius_b = 1.0 / sqrt(fabs(lambda2));
  return true;
}



bool Conic2D::EllipsoidSilhouetteToGauss(const double& ProbBorder,
                                         HomgPoint2D& Center,
                                         double& NormalizationFactor){

  const double  b = 0.5*((*this)[1][0]+(*this)[0][1]);
  const double  d = 0.5*((*this)[2][0]+(*this)[0][2]); 
  const double  e = 0.5*((*this)[2][1]+(*this)[1][2]); 
  
  const double dDisc = GetDiscriminant();
  if (fabs(dDisc) <= DBL_EPSILON)   {
    // degenerate case
    return false;
  }

  // find extremum of quadratic function
  Center[0] = (b * e - (*this)[1][1] * d) / dDisc;
  Center[1] = (b * d - (*this)[0][0] * e) / dDisc;
  Center[2] = 1;

  const double q0 = -LocatePoint(Center);
  (*this)[2][2] += q0;
 
  // if we want to evaluate exp( x^T * this * x) and get a probability
  // we have to normalize by 1.0 / (2 M_PI * sqrt( discriminant((-2.0*A)^-1 )))
  // where A is the upper left 2x2 part of this
  // after simplification and exploiting det(A^-1)=1/det(A)
  const double N = sqrt(GetDiscriminant()) / M_PI;

  const double scale = log(ProbBorder) / (-1.0*q0*(log(N)+1.0));
  (*this) *= scale;
  NormalizationFactor = sqrt(GetDiscriminant()) / M_PI;
  return true;
}

void Conic2D::SetQuadricProjection(const Quadric3D &Q, const PMatrix& P,
                                   bool UseSVD) {
  // get the dual quadric of Q since its projection is simple
  Matrix4x4<double> DualQuadric(Q.GetDualQuadric(UseSVD));
 
  // "project" the dual quadric to the dual conic
  Conic2D DualConic;
  DualConic = P * DualQuadric * P.Transpose();
  
  // get the actual conic from the dual conic
  (*this) = DualConic.GetDualConic(UseSVD);

  // set determinant to -1, so we can separate inside/outside in ellipses
  Normalize();
}

unsigned int Conic2D::GetSignature() const {
  // do singular value decomposition of this
  SVD svd((Matrix<CONIC2D_TYPE>)*this);
  Vector3<double> S;
  Matrix3x3<double> VT;
  S = svd.GetS();
  VT = svd.GetVT();
  // now we need the eigenvalues (with sign !)
  // compute this by projecting the (normalized) eigenvectors onto their images
  Vector3<double> eigenvector, eigenimage;
  int Signature = 0;
  for (unsigned int i=0; i<3; i++) {
    // get ith eigenvector and its image
    VT.GetRow(i,eigenvector);
    eigenimage = (*this) * eigenvector; 
    // compute scalar product, we are not "thinking" homogenized now
    // this is simply an analysis of a 3x3 matrix !
    S[i] = eigenimage * eigenvector;
    if (S[i]<-SVD_ZERO_THRESH) Signature--;
    else if (S[i]>SVD_ZERO_THRESH) Signature++;
  }
  
  // return number of eigenvalues with one sign minus number of eigenvalues
  // with opposite sign, choosing signs such that difference is not negative
  if (Signature<0) return (unsigned int)(-Signature);
  return (unsigned int)Signature;
}



bool Conic2D::IntersectsCircle(const HomgPoint2D& C, double Radius) const  {
  // code has been copied from maple, see below

  vector<double> Coefficients;
    
  const double r = Radius;
  const double u = C[0]/C[2];
  const double v = C[1]/C[2];
  
  const double a = (*this)[0][0];
  const double b = (*this)[1][1];
  const double c = (*this)[0][1] + (*this)[1][0];
  const double d = (*this)[0][2] + (*this)[2][0];
  const double e = (*this)[1][2] + (*this)[2][1];
  const double f = (*this)[2][2];

  // solve quartic equation
  Coefficients.resize(5);
    
  // coefficients copied from maple, see below
  Coefficients[4] = -2*a*b + b*b + a*a + c*c;   

  Coefficients[3] = 2*u*c*a + 2*u*c*b - 4*a*a*v + 2*d*c + 2*e*b + 4*b*a*v 
    - 2*v*c*c  - 2*a*e;

  Coefficients[2] = 2*f*b + 2*u*c*e - 2*a*a*r*r  + 2*a*a*u*u  + v*v*c*c  
    - r*r*c*c  + u*u*c*c  + e*e + 2*u*d*a + d*d  + 2*b*a*u*u 
    + 4*e*a*v - 2*a*f - 4*v*c*d - 4*u*c*a*v
    + 6*a*a*v*v  + 2*u*d*b - 2*b*a*v*v  + 2*b*a*r*r;

  Coefficients[1] = - 2*r*r*c*d - 4*a*a*u*u*v + 2*u*c*f - 2*e*a*v*v
    + 2*e*a*r*r  + 2*v*v*c*d + 2*f*e - 4*u*d*a*v + 4*f*a*v
    - 2*v*d*d + 2*u*u*u*c*a + 2*u*c*a*v*v - 2*u*c*a*r*r 
    + 2*e*a*u*u + 2*u*d*e + 4*a*a*v*r*r  - 4*a*a*v*v*v 
    + 2*u*u*c*d;

  Coefficients[0] =  2*u*d*f + 2*a*a*u*u*v*v - 2*a*a*u*u*r*r - 2*a*a*v*v*r*r
    + 2*u*u*u*d*a + 2*f*a*u*u - 2*f*a*v*v + 2*f*a*r*r + f*f 
    + a*a*u*u*u*u + a*a*v*v*v*v + a*a*r*r*r*r + u*u*d*d 
    + v*v*d*d - r*r*d*d + 2*u*d*a*v*v - 2*u*d*a*r*r;
  
     
  PolynomialSolve ps;
  ps.CheckCoefficients(Coefficients);
  return (ps.HasRealSolution(Coefficients));
  // solution is y component of point

/* maple code that has been used for IntersectsCircle:
   
eqns := { a*x^2+b*y^2+c*x*y+d*x+e*y+f=0, (x-u)*(x-u)+(y-v)*(y-v)=r*r };

                    2       2                              2      2      2
                  %1  b - %1  a + %1 e + 2 %1 a v + f - a u  - a v  + a r
  {y = %1, x = - --------------------------------------------------------}
                                       %1 c + d + 2 a u



                    2    2    2            4
     %1 := RootOf((b  + a  + c  - 2 a b) _Z  +

            2              2
    (- 2 v c  + 2 d c - 4 a  v + 2 u c b + 2 e b + 2 u c a + 4 b a v - 2 a e)

      3         2  2          2                2  2          2      2  2
    _Z  + (- 2 a  r  - 2 b a v  + 2 d a u + 2 a  u  + 2 b a u  + 6 a  v

        2  2                                  2                2  2          2
     + v  c  + 4 v a e + 2 u c e - 4 v d c + e  - 4 u c a v + u  c  + 2 b a r

                          2  2            2    2           2
     - 2 a f + 2 u d b - r  c  + 2 b f + d ) _Z  + (2 e a u  + 4 f a v

          2  3                    2            2        2                2
     - 4 a  v  + 2 f e + 2 u c a v  - 2 u c a r  - 2 v d  + 2 u d e + 2 v  c d

                      3                        2      2              2
     - 4 u d a v + 2 u  c a + 2 u c f + 2 e a r  - 2 r  c d - 2 e a v

          2          2  2        2    2                    3              2
     + 2 u  c d - 4 a  u  v + 4 a  v r ) _Z + 2 u d f + 2 u  d a - 2 f a v

        2      2  2  2    2  4    2  4    2  4    2  2    2  2    2  2
     + f  + 2 a  u  v  + a  u  + a  v  + a  r  + u  d  + v  d  - r  d

                2            2      2  2  2      2  2  2          2          2
     + 2 u d a v  - 2 u d a r  - 2 a  v  r  - 2 a  u  r  + 2 f a u  + 2 f a r )

*/

}

Conic2D Conic2D::GetDualConic(bool UseSVD) const {
  // for degenerate conics we MUST go through SVD
  if (!IsProper()) UseSVD = true;
  
  if (UseSVD) {
    // SVD needs a Matrix, but Conic2D is a Matrix3x3, so a little conversion is
    // needed here
    Matrix<CONIC2D_TYPE> m(*this);
    
    // do it the safe way
    SVD csvd(m, SVD_ZERO_THRESH, false); 
    unsigned int NullspaceDim = csvd.RelNullspaceDim();
    //cout<<"nullspace dim is "<<NullspaceDim<<endl;
    if (NullspaceDim==0) return (csvd.Invert());
    // we have a zero singular value in the svd and therefore a degenerate conic
    // return scaled adjugate matrix
    Matrix3x3<double> SM(MatrixZero);
    for (unsigned int i=NullspaceDim; i>0; i--) {
      SM[3-i][3-i] = 1.0;
    }
    //SM = csvd.GetV()*SM*csvd.GetU().Transpose();
    Matrix3x3<double> temp(csvd.GetV());
    temp *= SM;
    temp *= csvd.GetU().Transpose();
    SM = temp;
    
    const double thenorm = SM.NormFrobenius();
    if (thenorm< SVD_ZERO_THRESH) return Conic2D(SM);
    SM /= thenorm;
    return Conic2D(SM);
    
    
  }
  // do it the fast way
  const CONIC2D_TYPE& a = (*this)[0][0];
  const CONIC2D_TYPE& b = (*this)[0][1];
  const CONIC2D_TYPE& c = (*this)[0][2];
  const CONIC2D_TYPE& e = (*this)[1][1];
  const CONIC2D_TYPE& f = (*this)[1][2];
  const CONIC2D_TYPE& g = (*this)[2][2];
  // do it the fast way 
  Conic2D Dual;
  Dual[0][0] = e*g - f*f;
  Dual[0][1] = Dual[1][0] = c*f - b*g;
  Dual[0][2] = Dual[2][0] = b*f - c*e;
  Dual[1][1] = a*g - c*c;
  Dual[1][2] = Dual[2][1] = b*c - a*f;
  Dual[2][2] = a*e - b*b;

  return Dual;
}

int Conic2D::
GetTangentPoints(const HomgPoint2D& x, HomgPoint2D& p1, HomgPoint2D& p2) const
{
  BIASCDOUT(D_CONIC2D_TAN, "conic: "<<*this<<endl);
  BIASCDOUT(D_CONIC2D_TAN, "x: "<<x<<endl);
  Conic2D polar_cross, dc;
  HomgLine2D polar;

  // The polar of the pointy x with respect to the conic *this intersects the 
  // conic at the two points where the tangents to the conic through the point
  // x touch the conic.
  // See Hartley, Zisserman : "Multiple View Geometry", second edidtion
  // chapter 2.8, page 58
  //polar = (HomgLine2D)((*this)*x);
  Mult(x, polar);
  BIASCDOUT(D_CONIC2D_TAN, "polar: "<<polar<<endl);

  // dc is line (or dual) conic consisting of the two intersection points 
  // of the polar with the conic C
  polar_cross.SetAsCrossProductMatrix(polar);
  //dc = polar_cross * (*this) * polar_cross;
  Mult(polar_cross, dc);
  dc = (polar_cross * dc);
  // dc.MultLeft(polar_cross);
  BIASCDOUT(D_CONIC2D_TAN, "dc: "<<dc<<endl);

  // A dual conic dc consistig of the two points y=(y1, y2, 1)^T and 
  // x=(x1, x2, 1)^T is given by x*y^T+y*x^T 
  // (see Hartley, Zisserman : "Multiple View Geometry", second edidtion
  // chapter 2.2.3 "Conics and dual conics", page 32, Example 2.8)
  // resulting in the matrix A = 
  //                +-                                       -+
  //                |     2 x1 y1,    x1 y2 + x2 y1, x1 + y1  |
  //                |                                         |
  //                |  x1 y2 + x2 y1,    2 x2 y2,    x2 + y2  |
  //                |                                         |
  //                |     x1 + y1,       x2 + y2,       2     |
  //                +-                                       -+
  // The system given by (this) = A can be solved analytically:

  // Norm lower right entry of conic to 2.0
  //  dc /= 0.5*dc[2][2];
  //dc.MultiplyIP(2.0/dc[2][2]);
  dc*=(2.0/dc[2][2]);
  BIASCDOUT(D_CONIC2D_TAN,  "normed dc: "<<dc<<endl);

  // Set (*this) = +-         -+ 
  //               |  a  b  d  |
  //               |  b  c  e  |
  //               |  d  e  2  |
  //               +-         -+
  double a=dc[0][0], b=dc[0][1], c=dc[1][1], d=dc[0][2], e=dc[1][2];
  // Setting (*this) = A results in the equations 
  // e = x2+y2  ->  x2 = e-y2   (I)
  // d = x1+y1  ->  x1 = d-y1   (II)
  // c = 2*x2*y2                (III)
  // b = x1*y2 + x2*y1          (IV)
  // a = 2*x1*y1                (V)
  //
  double y1a, y1b, y2a, y2b, y1, y2, min;
  // Now use (I) in (III) and solve the quadratic equation
  double tmp=e*e*0.25 - c*0.5, tmp2;
  if (tmp>=0.0){
    tmp = sqrt(tmp);
    tmp2=e*0.5;
    y2a = tmp2+tmp;
    y2b = tmp2-tmp;
  } else {
    BIASERR("no solution for y2: tmp<0 "<<tmp);
    return -1;
  }
  BIASCDOUT(D_CONIC2D_TAN, "y2a: "<<2.0*(e-y2a)*y2a-c
            <<"\ny2b: "<<2.0*(e-y2b)*y2b-c<<endl);

  // Now use (II) in (V) and solve the quadratic equation
  tmp=d*d*0.25 - a*0.5;
  if (tmp>=0.0){
    tmp = sqrt(tmp);
    tmp2 = d*0.5;
    y1a = tmp2+tmp;
    y1b = tmp2-tmp;
  } else {
    BIASERR("no solution for y1: tmp<0 "<<tmp);
    return -1;
  }
  BIASCDOUT(D_CONIC2D_TAN,  "y1a: "<<2*d*y1a-2*y1a*y1a-a
            << "\ny1b: "<<2*d*y1b-2*y1b*y1b-a<<endl);

  // 4 different solutions exist. Test for the best solution by using
  // the equation (IV) and minimize the error

  // test y1a, y2a
  BIASCDOUT(D_CONIC2D_TAN,  "y1a, y2a ("<<y1a<<", "<<y2a<<")"<<endl);
  y1=y1a, y2=y2a;
  min = fabs((d-y1)*y2+(e-y2)*y1 - b);
  BIASCDOUT(D_CONIC2D_TAN,  "y1a, y2a = "<<min<<endl);
  // test y1a y2b
  BIASCDOUT(D_CONIC2D_TAN,  "y1a, y2b ("<<y1a<<", "<<y2b<<")"<<endl);
  tmp = fabs((d-y1a)*y2b+(e-y2b)*y1a - b);
  if (tmp<min){
    min = tmp;
    y1=y1a;
    y2=y2b;
    BIASCDOUT(D_CONIC2D_TAN,  "y1a, y2b = "<<min<<endl);
  } else {
    BIASCDOUT(D_CONIC2D_TAN,  "not taking y1a, y2b = "<<tmp<<endl);
  }
  // test y1b, y2a
  BIASCDOUT(D_CONIC2D_TAN,  "y1b, y2a ("<<y1b<<", "<<y2a<<")"<<endl);
  tmp = fabs((d-y1b)*y2a+(e-y2a)*y1b - b);
  if (tmp<min){
    min = tmp;
    y1=y1b;
    y2=y2a;
    BIASCDOUT(D_CONIC2D_TAN,  "y1b, y2a = "<<min<<endl);
  } else {
    BIASCDOUT(D_CONIC2D_TAN,  "not taking y1b, y2a = "<<tmp<<endl);
  }

  // test y1b, y2b
  BIASCDOUT(D_CONIC2D_TAN,  "y1b, y2b ("<<y1b<<", "<<y2b<<")"<<endl);
  tmp = fabs((d-y1b)*y2b+(e-y2b)*y1b - b);
  if (tmp<min){
    min = tmp;
    y1=y1b;
    y2=y2b;
    BIASCDOUT(D_CONIC2D_TAN,  "y1b, y2b = "<<min<<endl);
  } else {
    BIASCDOUT(D_CONIC2D_TAN,  "not taking y1b, y2b = "<<tmp<<endl);
  }

  p1.Set(y1, y2, 1.0);
  p2.Set(d-y1, e-y2, 1.0);

  BIASCDOUT(D_CONIC2D_TAN, "tangent points: "<<p1<<"\t"<<p2<<endl);
    

  return 2;
}

int Conic2D::
GetLines(std::vector<HomgLine2D>& lines) const {
  lines.clear();
  Conic2D A(*this);
  A /= A.NormFrobenius();
  ConicType thetype = A.GetConicType();
  if (thetype==DoubleLine) {
    // in principal we can use getdualconic, however, we get an unknown
    // scale, therefore we here compute the adjugate directly:
    //   adjugate matrix (NOT to be confused with 
    //   adjoint or with moore-penrose-pseudo-inverse) 
    // first compute cofactor matrix
    Matrix3x3<double> cofactors;
    for (unsigned int i=0; i<3; i++) {
      for (unsigned int j=0; j<3; j++) {
        Matrix2x2<double> submatrix((*this)[(i+1)%3][(j+1)%3],
                                    (*this)[(i+1)%3][(j+2)%3],
                                    (*this)[(i+2)%3][(j+1)%3],
                                    (*this)[(i+2)%3][(j+2)%3]);
        cofactors[i][j] = submatrix.det();
      }
    }
    Conic2D Dual(cofactors.Transpose());

    //SVD testsvd(Dual);
    //cout<<"dual has singular values "<<testsvd.GetS()<<endl;

    double themax = 0;
    int index = -1;
    for (unsigned int i=0; i<3; i++) {
      if (fabs(Dual[i][i])>themax) {
        themax = fabs(Dual[i][i]);
        index = i;
      }
    }
    if (index<0) {
      BIASERR("didnt find positive diagonal element");
      return -1;
    }
    if (Dual[index][index]<0.0) {
      A *= -1.0;
      Dual *= -1.0;
    }
    double beta = sqrt(themax);
    //cout<<"Dual is "<<Dual<<" while beta is "<<beta<<endl;
    Vector3<double> p(Dual[index][0]/beta,
                      Dual[index][1]/beta,
                      Dual[index][2]/beta); 

    //cout<<"p is "<<p<<endl;
    
    // this results in error: no match for 'operator+' (rwulff)
    //Matrix3x3<double> C((*this) + p.GetSkewSymmetricMatrix());
    Matrix3x3<double> C(*this);
    C += p.GetSkewSymmetricMatrix();


    //SVD mysvd(C);
    //cout<<"singular values of C are "<<    mysvd.GetS()<<endl;
    //cout<<"C is "<<C<<endl;
    HomgLine2D g(0,0,0),h(0,0,0);
    for (unsigned int i=0; i<3; i++) {
      for (unsigned int j=0; j<3; j++) {
        if (fabs(C[i][j])>1e-10) {
          // found nonzero element:
          g[0] = C[i][0];
          g[1] = C[i][1];
          g[2] = C[i][2];
          h[0] = C[0][j];
          h[1] = C[1][j];
          h[2] = C[2][j];
          break;
        }
      }
    }
    g /= g.NormL2();
    lines.push_back(g);
    h /= h.NormL2();
    lines.push_back(h);
  } else if (thetype==SingleLine) {
    HomgLine2D g(0,0,0);
    if ((A[0][0]<=0.0) && (A[1][1]<=0.0) && (A[1][1]<=0.0)) {
      A *= -1.0;
    }
    for (unsigned int i=0; i<3; i++) {
      if (A[i][i]>=0.0) g[i] = sqrt(A[i][i]);
      else if (A[i][i]>-1e-5) g[i] = 0;
      else {
        //g[i] = 0;
        BIASERR("line has complex coefficients, need sqrt from "<<A[i][i]);
        return -1;
      }
    }
    //cout<<"g absolute values are "<<g<<endl;
    // the absolute values of g are correct, however, we must check the signs

    Matrix<double> Aabsolute(g.OuterProduct(g)-A);
    //cout<<"Aabsolute is "<< Aabsolute <<endl;


    // it seems more reasonable to check for positivity here than for <1e-10!!!

    // bool signabequal = (Aabsolute[0][1]<1e-10);
    //bool signbcequal = (Aabsolute[1][2]<1e-10);
    //bool signacequal = (Aabsolute[0][2]<1e-10);

    bool signabequal = (Aabsolute[0][1]>0.0);
    bool signbcequal = (Aabsolute[1][2]>0.0);
    bool signacequal = (Aabsolute[0][2]>0.0);

    if (signabequal) {
      if (signbcequal) {
        if (signacequal) {
          // fine, all positive or all negative!
        } else {
          BIASERR("inconsistent data: "<<Aabsolute[0][1]<<" "
                  <<Aabsolute[1][2]<<" "<<Aabsolute[0][2]);
        }
      } else {
        if (signacequal) {
          BIASERR("inconsistent data: "<<Aabsolute[0][1]<<" "
                  <<Aabsolute[1][2]<<" "<<Aabsolute[0][2]);
        } else {
          g[2] *= -1.0;
        }
      }
    } else {
      if (signbcequal) {
        if (signacequal) {
          BIASERR("inconsistent data: "<<Aabsolute[0][1]<<" "
                  <<Aabsolute[1][2]<<" "<<Aabsolute[0][2]);
        } else {
          g[0] *= -1.0;
        }
      } else {
        if (signacequal) {
          g[1] *= -1.0;
        } else {
          BIASERR("inconsistent data: "<<Aabsolute[0][1]<<" "
                  <<Aabsolute[1][2]<<" "<<Aabsolute[0][2]);
        }
      }
    }
    //cout<<"g final is "<<g<<endl;

    lines.push_back(g);
  } else {
    BIASERR("GetLines called for Conic which does not consist of one or two lines! Type is "<<thetype);
    BIASABORT;
    return -1;
  }
  return 0;
}

int Conic2D::
IntersectConic(const Conic2D& otherconic,
               std::vector<HomgPoint2D>& intersectionPoints) const {
  intersectionPoints.clear();
  if (!IsProper()) {
    //cout<<"improper conic has type "<<GetConicType()<<endl;
    if (otherconic.IsEmpty()) return 0; // no solution at all
    switch (GetConicType()) {
    case WholePlane: {
      if (otherconic.IsPoint()) {
        HomgPoint2D solution;
        if (otherconic.GetPoint(solution)!=0) return 1;
        intersectionPoints.push_back(solution);
        return 0;
      }  // no solution
      return 1; // degenerate case with multiple solutions
    }
    case SingleLine:
    case DoubleLine: {
      vector<HomgLine2D> thelines;
      GetLines(thelines);
      if (otherconic.IsProper()) {
        vector<HomgPoint2D> thepoints;
        for (unsigned int i=0; i<thelines.size(); i++) {
          otherconic.IntersectLine(thelines[i], thepoints);
          for (unsigned int j=0; j<thepoints.size(); j++) {
            thepoints[j].Normalize();
            bool isNewPoint = true;
            for (unsigned int k=0; k<intersectionPoints.size(); k++)
              if (Equal(thepoints[j][0], intersectionPoints[k][0]) &&
                  Equal(thepoints[j][1], intersectionPoints[k][1]) &&
                  Equal(thepoints[j][2], intersectionPoints[k][2])) 
                isNewPoint = false;
            if (isNewPoint) intersectionPoints.push_back(thepoints[j]);
          }
        }
        return 0;
      } else {
        // both conics are degenerate !
        switch (otherconic.GetConicType()) {
        case SingleLine:
        case DoubleLine: {
          vector<HomgLine2D> otherlines;
          otherconic.GetLines(otherlines);
          // now intersect each line with each other
          for (unsigned int i=0; i<thelines.size(); i++) {
            for (unsigned int j=0; j<otherlines.size(); j++) {
              HomgPoint2D intersection = 
                thelines[i].CrossProduct(otherlines[j]);
              const double norm = intersection.NormL2();
              if (norm<1e-10) continue; // identical lines
              intersection /= norm;
              bool isNewPoint = true;
              for (unsigned int k=0; k<intersectionPoints.size(); k++)
                if (Equal(intersection[0], intersectionPoints[k][0]) &&
                    Equal(intersection[1], intersectionPoints[k][1]) &&
                    Equal(intersection[2], intersectionPoints[k][2])) 
                  isNewPoint = false;
              if (isNewPoint) intersectionPoints.push_back(intersection);
            }
          }
          return 0;
        }
        case Point: {
          HomgPoint2D point;
          if (otherconic.GetPoint(point)!=0) return -1;
          if (fabs(LocatePoint(point))>1e-9) return 0;
          intersectionPoints.push_back(point);
          return 0;
        }
        default: return -1; // degenerate case with multi-solutions
        }
      }
    }
    case Point: {
      HomgPoint2D point;
      if (GetPoint(point)!=0) return -1;
      if (fabs(otherconic.LocatePoint(point))>1e-9) return 0;
      intersectionPoints.push_back(point);
      return 0;
    }
    case Empty: return 0; // no solution, everything fine
    default: return -1;
    }
  } else if (!otherconic.IsProper()) {
    // switch roles of conics and use upper part in other conic object ...
    return otherconic.IntersectConic(*this, intersectionPoints);
  } 
  // intersection of two proper conics:
  return IntersectConicProper(otherconic, intersectionPoints);
}


int Conic2D::
IntersectConicProper(const Conic2D& otherconic,
                     std::vector<HomgPoint2D>& intersectionPoints) const {

  BIASASSERT(IsProper()); // read below
  BIASASSERT(otherconic.IsProper()); // read below
  // this implementation handles the difficult case of intersecting 
  // ellipses/parabolas/hyberbolas with poissibly 4 solutions
  // if one or both of the conics are actually lines or single points
  // this is handled by the intersectconic method
  intersectionPoints.clear();
 

  // the intersection points must lie on both conics:
  // x C1 X = 0            x C2 x = 0
  // therefore they must also fulfill
  // x    (l C1 + m C2)     x = 0
  // for ANY l and m
  // particularly, if a and b are chosen in a way that the conic degenerates
  // det(l C1 + m C2) = 0
  // this happens when the characteristic polynomial vanishes.


  // actually this is the same as in the 7point algorithm for f-estimation
  FMatrixEstimation FEst;
  
  BIAS::Vector<double> f1(9), f2(9);
  for (unsigned int i=0; i<9; i++) {
    f1[i] = (*this)[i/3][i%3];
    f2[i] = otherconic[i/3][i%3];
  }
  vector<double> polynomial;
  FEst.GetDetPolynomial(f1,f2,polynomial);
  PolynomialSolve PS;
  std::vector<double > solutions;
  //cout<<"Polynomial is ";
  //for (unsigned int i=0; i<polynomial.size(); i++) 
  //  cout<<polynomial[i]<<" ";
  //cout<<endl;
  if (PS.Solve(polynomial, solutions)<1) {
    BIASERR("PS failed.");
    return -1;
  }
  if (solutions.empty()) {
    //cout<<"Polynomial has no real root!!!"<<endl;
    return -1;
  }


  // should we consider the other solutions here as well or maybe select a 
  // "better" solution
  Conic2D degenerated(solutions[0]*(*this) + (1.0-solutions[0])*otherconic);
  const double  degeneratednorm = degenerated.NormFrobenius();
  if (degeneratednorm<1e-10) return 1;
  degenerated /=  degeneratednorm;

 //  SVD mysvd(degenerated);
//   Vector<double> S(mysvd.GetS());
//   Matrix<double> SS(3,3,MatrixZero);
//   SS[0][0] = S[0];
//   SS[1][1] = S[1];
//   cout<<"det is "<<degenerated.det()<<endl;
//   degenerated = mysvd.GetU() * SS * mysvd.GetVT();
//   cout<<"det is now "<<degenerated.det()<<endl;
//   Matrix<double> mymat(degenerated);
//   cout<<"general det is "<<mymat.DetSquare()<<endl;
//   SVD mysvd2(degenerated);
//   cout<<"degenerated conic has singular values "<<mysvd2.GetS()<<endl;

  vector<HomgLine2D> lines;
  degenerated.GetLines(lines);
  for (unsigned int i=0; i<lines.size(); i++) {
    vector<HomgPoint2D> points;
    this->IntersectLine(lines[i],points);
    //cout<<"this line intersects the conic in "<<points.size()<<" points: "<<endl;
   



    for (unsigned int j=0 ;j<points.size(); j++) {
      //cout<<points[j]<<endl;
      points[j].Normalize();
      bool isNewPoint = true;
      for (unsigned int k=0; k<intersectionPoints.size(); k++)
        if (Equal(points[j][0], intersectionPoints[k][0]) &&
            Equal(points[j][1], intersectionPoints[k][1]) &&
            Equal(points[j][2], intersectionPoints[k][2])) isNewPoint = false;
      if (isNewPoint) intersectionPoints.push_back(points[j]);
    }
  }

  return 0;
}

int Conic2D::
IntersectLine(const HomgLine2D& theline,
              std::vector<HomgPoint2D>& intersectionPoints) const {
  intersectionPoints.clear();
  if (theline.NormL2()<1e-10) return -1;

  HomgLine2D myline(theline);
  myline /= myline.NormL2();
  //cout<<"using normalized line "<<myline<<endl;
  Matrix3x3<double> A(*this);

  double themax = 0;
  int theindex = -1;
  for (unsigned int i=0; i<3; i++) {
    if (fabs(myline[i])>themax) {
      themax = fabs(myline[i]);
      theindex = i;
    }
  }
  //  cout<<"largest line element is "<<theindex<<endl;

  if (theindex<0) {
    BIASERR("Supplied invalid line 000");
    return -1;
  }
  Matrix3x3<double> M(myline.GetSkewSymmetricMatrix());
  //cout<<"M is "<<M<<endl;
  Matrix3x3<double> B(M.Transpose() * A * M);
  //cout<<"B is "<<B<<endl;
  Matrix2x2<double> submatrix(B[(theindex+1)%3][(theindex+1)%3],
                              B[(theindex+1)%3][(theindex+2)%3],
                              B[(theindex+2)%3][(theindex+1)%3],
                              B[(theindex+2)%3][(theindex+2)%3]);
  //cout<<"submatrix is "<<submatrix<<endl;
  double det = submatrix.det();
  if (det<0.0) {
    //cout<<"negative det., changing signs "<<endl;
    A *= -1.0;
    B *= -1.0;
    det *= -1.0;
  }
  double alpha = sqrt(det)/myline[theindex];
  //cout<<"alpha is "<<alpha<<endl;

  Matrix3x3<double> C(B - alpha*M);
  //cout<<"C is "<<C<<endl;
  HomgPoint2D p,q;
  bool foundit = false;
  for (unsigned int i=0; i<3; i++) {
    for (unsigned int j=0; j<3; j++) {
      if (fabs(C[i][j])>1e-10) {
        // found nonzero element:
        p[0] = C[i][0];
        p[1] = C[i][1];
        p[2] = C[i][2];
        q[0] = C[0][j];
        q[1] = C[1][j];
        q[2] = C[2][j];
        foundit = true;
        break;
      }
    }
  }
  if (!foundit) return -2;
  //cout<<"p is "<<p<<endl;
  //cout<<"q is "<<q<<endl;

  p.Homogenize();
  intersectionPoints.push_back(p);
  q.Homogenize();
  intersectionPoints.push_back(q);
  return 0;
}


void Conic2D::
SetSingleLine(HomgLine2D theline) {
  theline /= theline.NormL2();
  (*this) = theline.OuterProduct(theline);
}


void Conic2D::
SetTwoLines(HomgLine2D theline1,
            HomgLine2D theline2) {
  theline1 /= theline1.NormL2();
  theline2 /= theline2.NormL2();
  (*this) = theline1.OuterProduct(theline2) + theline2.OuterProduct(theline1);
}

 
int  Conic2D::
GetLinearizedOffset(const HomgPoint2D& point,
                    HomgPoint2D& distance,
                    const HomgPoint2DCov& pointcov) const {
  BIASERR("untested");
  BIASABORT;
  HomgPoint2D p(point);
  HomgPoint2D n(*this * p);
  const double norm = n.NormL2();
  if (norm<1e-9) return -1;
  n *= 1.0 / norm;

  pointcov.Mult(n, distance);
  distance *= n.ScalarProduct(point) / (2.0 * n.ScalarProduct(distance));
  
  return 0;
}

 
double Conic2D::
GetPointDistance(const HomgPoint2D& point,
                 HomgPoint2D& distance,
                 const HomgPoint2DCov& pointcov) const {
  BIASERR("untested");
  BIASABORT;
  if (GetLinearizedOffset(point, distance, pointcov)!=0){
    BIASERR("could not estimate distance");
    return DBL_MAX;
  }
  double l = LocatePoint(point);
  return (l*l / (4.0 * 
                 point.ScalarProduct((*this) * pointcov * (*this) * point)));


}