Conic2D.hh

/* 
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
*/


#ifndef __CONIC2D_HH__
#define __CONIC2D_HH__

#include <bias_config.h>
#include <Base/Common/W32Compat.hh>
#include <math.h>

#include <Base/Debug/Error.hh>
#include <Base/Debug/Debug.hh>
#include <Base/Math/Matrix3x3.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Base/Geometry/HomgPoint2DCov.hh>
#include <Base/Geometry/HomgLine2D.hh>

#define CONIC2D_TYPE double

#define D_CONIC2D_TAN 0x00000001

#define GAUSS2D_CONFIDENCE_20_PERCENT 0.66
#define GAUSS2D_CONFIDENCE_39_PERCENT 1.00
#define GAUSS2D_CONFIDENCE_50_PERCENT 1.18
#define GAUSS2D_CONFIDENCE_68_PERCENT 1.52
#define GAUSS2D_CONFIDENCE_90_PERCENT 2.15
#define GAUSS2D_CONFIDENCE_95_PERCENT 2.45
#define GAUSS2D_CONFIDENCE_99_PERCENT 3.04

namespace BIAS {

  // forward declarations, to reduce compile time, includes are in the cc file
  template <class T> class Matrix2x2;
  template <class T> class Image;
  class Quadric3D;
  class PMatrix;

  /** @class Conic2D
      @ingroup g_geometry
      @brief A 3x3 matrix representing a conic (cone/plane intersection)
      
      A conic is a (contour) line in P2, which resulted from the projection
      of a quadric (see Quadric3D) of P3. Conics are ellipses, hyperbolas, ... 
      Each point x of the conic C fulfills  x^T C x = 0
      which is a quadratic equation in the components of x.
      If nothing else is mentioned, we mean point conics, not line conics.
      @author koeser, 10/2003 */

  class BIASGeometry_EXPORT Conic2D 
    : public Matrix3x3<CONIC2D_TYPE>, public Debug
  {
  public:

    enum ConicType { Unknown=0, WholePlane, Ellipse, Parabola, Hyperbola,
                     SingleLine, DoubleLine, Point, Empty};

    /** @brief default constructor sets all fields to zero */
    Conic2D() {};

    /** @brief set default matrix values */
    explicit Conic2D(const MatrixInitType& i)
      : Matrix3x3<CONIC2D_TYPE>(i){};

    /** @brief copy constructor uses Conic2D::operator= */
    Conic2D(const Matrix3x3<CONIC2D_TYPE>& m) 
    { Matrix3x3<CONIC2D_TYPE>::operator=(m); };      
     
    /** @brief copy constructor uses Conic2D::operator= */   
    Conic2D(const Matrix<CONIC2D_TYPE>& m)   
    { Matrix3x3<CONIC2D_TYPE>::operator=((Matrix3x3<CONIC2D_TYPE>)m); };

    /** @brief check whether this conic is an ellipse */
    bool IsEllipse() const;

    /** @brief check whether this conic is a parabola */
    bool IsParabola() const;

    /** @brief check whether this conic is a hyperbola */
    bool IsHyperbola() const;

    /** @brief check whether this conic is a single line */
    bool IsSingleLine() const;

    /** @brief check whether this conic is a double line */
    bool IsDoubleLine() const;

    /** @brief check whether this conic is a point */
    bool IsPoint() const;

    /** @brief check whether this conic contains no points */
    bool IsEmpty() const;

    /** @brief is this a proper conic (parabola/hyperbola/ellipse) with rank3
        or some of the degenerate cases ? */
    bool IsProper() const;

    /** @brief return the type of point conic of (*this) */
    ConicType GetConicType() const;

    /** @brief constructs a conic from a point and a covariance matrix
        @author woelk 06/2005 */
    void SetPointAndCovariance(const HomgPoint2D& center, 
                               const Matrix2x2<CONIC2D_TYPE>& cov,
                               const double& dScale = 
                               GAUSS2D_CONFIDENCE_39_PERCENT);

    /** @brief reconstructs center and covariance matrix from conic 
        @author woelk 06/2005 */
    bool GetPointAndCovariance(HomgPoint2D& center,
                               Matrix2x2<CONIC2D_TYPE>& cov) const;

    /** @brief determines if the point is inside/on/outside the conic

    for "closed" conics, such as ellipses, call this function for
    a point to determine its position relative to the conic:
    <0 means: inside the conic
    >0 means: outside the conic
    =0 means on the border line
    @attention: make sure that the conic is normalized
    LocatePoint internally computes the difference of Mahalanobis distances
    of point2D from the center and a conic point from the center */
    inline double LocatePoint(const HomgPoint2D& point2D) const {
      return ( point2D *((*this)*point2D) ); }
      
    /** @brief sets mean of first 2 diag elements to positive value, 
        so that LocatePoint(x)<0 means x is in conic */
    inline bool Normalize() { double d=(*this)[0][0]+(*this)[1][1];  
    if (fabs(d)<DOUBLE_EPSILON) return false; (*this) /= d; return true;}

    /** @brief construct an ellipse with explicit parameters
        @param radius_a radius in (originally) x direction
        @param radius_b radius in (originally) y direction
        @param dAngle rotation angle in radiant (math. positive)
        @center center of ellipse  */
    void SetEllipse(const HomgPoint2D& Center, const double& dAngle,
                    const double& radius_a, const double& radius_b);

    /** @brief create a degenerate point conic consisting only of one line */
    void SetSingleLine(HomgLine2D theline);

    /** @brief create a degenerate point conic consisting of two lines */
    void SetTwoLines(HomgLine2D theline1,
                     HomgLine2D theline2);

    /** @brief computes explicit ellipse representation
        
    if the given conic is an ellipse, the ellipse parameters are extracted
    and returned in the functions parameters
    @param dAngle rotation angle in radiant (math. positive)
    @param radius_a radius in (originally) x direction
    @param radius_b radius in (originally) y direction
    @return returned parameters are only valid if true is returned,
    otherwise (*this) is no (or a degenerate) ellipse */
    bool GetEllipseParameters(HomgPoint2D& Center, 
                              double& dAngle,
                              double& radius_a, 
                              double& radius_b) const;

    /** returns the center and the two halfaxes of the conic if feasible
        (f.e. for drawing with the class ImageDraw)
        @author woelk 06/2005 */
    bool GetEllipseParameters(double center[2], 
                              double a[2], double b[2]) const;

    /** @brief retrieve one or two lines from a degenerate point conic
        @return 0 lines contains all lines, other values indicate an error, 
        e.g. the conic does not consist of two lines */
    int GetLines(std::vector<HomgLine2D>& lines) const;

    /** @brief if the conic consists of a single point (degenerate case, rank=2)
     *         retrieve this point */
    int GetPoint(HomgPoint2D& thepoint) const;

    /** @brief assign a probability of a gaussian distribution to a conic to
        measure probabilities in the image plane

        Ellipsoids in 3D space represent iso surfaces of gaussian normal 
        distributions (e.g. of the 99% volume). Projecting a silhouette of such
        an ellipsoid gives a conic (e.g. an ellipse) in the image.
        If we want to interpret the conic as the covariance matrix of a gauss
        within the image plane, we have to convert it in such a way that
        the ellipse center yields zero and the border (the former conic line) 
        a different value than zero, e.g. a value corresponding to 99%. 

        Afterwards, you can simply compute
        NormalizationFactor * exp(this->LocatePoint(x))
        to compute the probability at x

        @status still have to test this !
    */
    bool EllipsoidSilhouetteToGauss(const double& ProbBorder,
                                    HomgPoint2D& Center,
                                    double& NormalizationFactor);

    /** @brief draws conic into an image using a brute force method 
     *
     *   Runs across all pixels and checks for zero crossings of LocatePoint.
     *   Really slow, but works for ANY type. If the conic is an ellipse,
     *   a more elegant way of drawing can be found in ImageDraw
     */
    void Draw(Image<unsigned char>& img) const;

    Conic2D& operator=(const Matrix<CONIC2D_TYPE>& m) {
      Matrix3x3<CONIC2D_TYPE>::operator=(m);
      return (*this);
    };

    /** @brief constructs a conic that is a projection of the quadric Q using
        camera matrix P */
    void SetQuadricProjection(const Quadric3D &Q, const PMatrix& P,
                              bool UseSVD = false);

    /** @brief returns true if the given Conic intersects with the circle of
        radius Radius around point C, false otherwise

        This is a contour line intersection ! Does not test whether circle
        lies "in" conic or vice versa ! 
        @note strongly depends upon numerical robustness of SolveQuartic !
        You might as well try IntersecConic() to obtain the solutions
        @status tested */
    bool IntersectsCircle(const HomgPoint2D& C, double Radius) const;
    
    /** @brief returns the dual of this conic, which is the inverse */
    Conic2D GetDualConic(bool UseSVD = false) const;

    /** @brief computes signature of matrix, 
        (no. of neg. eigenvalues) - (no. of pos. eigenvalues) or vice versa
        if there are more pos. eigenvalues */
    unsigned int GetSignature() const;

    /** @brief Returns the two points p1 and p2 on the conic subject to the 
        constraint that the two tangents at the points p1 and p2 intersect in 
        the point x.
        Returns the number of tangent points (1 or 2) or -1 on error
        @author woelk 06/2005 */
    int GetTangentPoints(const HomgPoint2D& x, HomgPoint2D& p1, 
                          HomgPoint2D& p2) const;

    /** @brief compute intersection of 2 non-degenerate (full-rank) conics
     *  @author koeser 02/2008 
     *  @result 0:      intersectionPoints contains all intersections,
     *          other:  degenerate case or conics are equal
     */
    int IntersectConicProper(const Conic2D& otherconic,
                             std::vector<HomgPoint2D>& intersectionPoints) 
      const;

    /** @brief compute intersection of 2 conics
     *  @author koeser 02/2008 
     *  @result 0:      intersectionPoints contains all intersections,
     *          other:  degenerate case (infinite solutions)
     */
    int IntersectConic(const Conic2D& otherconic,
                       std::vector<HomgPoint2D>& intersectionPoints) const;

    /** @brief compute intersection of a conic with a line
     *  @author koeser 02/2008 
     *  @result 0:      intersectionPoints contains all intersections,
     *          other:  degenerate case or conics are equal
     */
    int IntersectLine(const HomgLine2D& theline,
                      std::vector<HomgPoint2D>& intersectionPoints) const;

   
    /** @brief compute approximate offset of a measured point to a known conic
     *
     *  This uses the linearization of a quadratic equation under the 
     *  assumption that the point is close to the conic. With increasing
     *  outside distance to the conic, the offset is underestimated.
     *  See [Kanatani96:Statistical Optimization..., p.169]
     *  @author koeser 02/2009 */
    int GetLinearizedOffset(const HomgPoint2D& point,
                            HomgPoint2D& distance,
                            const HomgPoint2DCov& pointcov = 
                            HomgPoint2DCov(MatrixIdentity)) const;

    /** @brief return the squared mahalanobis distance of the point to the conic
     *
     *  This function can be used for the decision whether a noisy point lies
     *  on a conic curve or not (e.g. threshold the result at 4.0).
     *  @return If point is an observation from a normal distribution with its 
     *          mean on the conic curve than the resulting squared mahalanobis
     *          distance is 1D chi-squared distributed. This means e.g. less 
     *          than 5% of all such points will have a distance larger than 4
     *          (this is the "2-sigma-band").
     *          
     *  @author koeser */
    double GetPointDistance(const HomgPoint2D& point,
                            HomgPoint2D& distance,
                            const HomgPoint2DCov& pointcov = 
                            HomgPoint2DCov(MatrixIdentity)) const;

    

  protected:
    /** @return a value classifying the conic: 
        -   Discriminant<0: hyperbola 
        -  (Discriminant=0: parabola ?) 
        -   Discriminant>0: ellipse */
    double GetDiscriminant() const;
  };
  
  inline std::ostream& operator<<(std::ostream &os, Conic2D::ConicType& t) {
    switch (t) {
    case Conic2D::Ellipse   :os<<"Ellipse";break;
    case Conic2D::Parabola  :os<<"Parabola";break;
    case Conic2D::Hyperbola :os<<"Hyperbola";break;
    case Conic2D::SingleLine:os<<"SingleLine";break;
    case Conic2D::DoubleLine:os<<"DoubleLine";break;
    case Conic2D::Point     :os<<"Point";break;
    case Conic2D::Empty     :os<<"Empty";break;
    case Conic2D::WholePlane:os<<"WholePlane";break;
    case Conic2D::Unknown   :os<<"Unknown";break;
    default:BIASERR("Undefined ConicType !"); BIASABORT;
    }
    return os;
  }

}
#endif