Quadric3D.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 __QUADRIC3D_HH__
#define __QUADRIC3D_HH__
#include "bias_config.h" 

#include <Base/Debug/Error.hh>
#include <Base/Debug/Debug.hh>
#include <Base/Math/Matrix4x4.hh>
#include <Base/Geometry/HomgPoint3D.hh>
#include <Geometry/CovMatrix3x3.hh>

#define QUADRIC3D_TYPE double

// defines for confidence of new inlying measurement, see text below
#define GAUSS3D_CONFIDENCE_20_PERCENT 1.00
#define GAUSS3D_CONFIDENCE_39_PERCENT 1.36
#define GAUSS3D_CONFIDENCE_50_PERCENT 1.54
#define GAUSS3D_CONFIDENCE_68_PERCENT 1.88
#define GAUSS3D_CONFIDENCE_90_PERCENT 2.50
#define GAUSS3D_CONFIDENCE_95_PERCENT 2.79
#define GAUSS3D_CONFIDENCE_99_PERCENT 3.37



namespace BIAS {

  /** @class Quadric3D
      @ingroup g_geometry
      @brief A quadric as a 4x4 matrix describing a surface in 3d projective
             space defined by a quadratic equation, e.g. a sphere.

      Quadric3D is a 4x4 matrix Q, which describes a surface in 3d.
      All points x that lie on this surface, fulfill x^T Q x = 0,
      which is a quadratic equation in the components of x.
      For quadrics such as spheres or ellipsoids you can check if
      a point is inside or outside the quadric using LocatePoint(),
      for others you can also get the "side" the point lies relative to the
      Quadric.
      If the Quadric is a _normalized_ ellipsoid, points inside yield negative 
      values while LocatePoint computes positive ones for points ouside.

      Projective cameras map quadrics to conics (see Conic2D) in the image
      plane.
      
      Construction of Quadrics (i.e. ellipsoids) from covariance matrices:
      You can construct a Quadric3D from a CovMatrix3x3, the 3d surface
      will then represent all points of some constant probability of the
      3d-normal distribution. The decision which probability to choose
      as a threshold, and thus how large the ellipsoid should become,
      depends on the desired probability for a new measurement
      to lie inside the quadric.

      The standard confidence region is the set of points which lie inside
      the ellipsoid defined by main axes' length = sigma_i
      The probability that a measurement falls into this region is only 20%.
      If you want to increase the confidence that a point falls into your
      ellipsoid, you may enlarge the main axes by multiplication with a 
      constant factor (see 3D column in subsequent table).
      For comparison purposes, the coresponding factors for 1D/2D are also
      given in the table:


      -          1-D  2-D  3-D  Conf(%)
      -         0.01 0.14 0.33 .. 01
      -         0.06 0.32 0.59 .. 05
      -         0.14 0.46 0.76 .. 10
      -         0.25 0.66 1.00 .. 20
      -         0.52 1.00 1.36 .. 39
      -         0.68 1.18 1.54 .. 50
      -         1.00 1.52 1.88 .. 68
      -         1.64 2.15 2.50 .. 90
      -         1.96 2.45 2.79 .. 95
      -         2.58 3.04 3.37 .. 99
      -         2.81 3.26 3.58 .. 99.5

      Suppose you have a covariance matrix and want to create an ellipsoid,
      where 99% of new measurements will fall in. In that case you have
      to use main axes of length (sigma_i*3.37)

      @author koeser 10/2003 */

  class BIASGeometry_EXPORT Quadric3D : public Matrix4x4<QUADRIC3D_TYPE>
  {

  public:
    Quadric3D() {};

    /** @brief set default matrix values */
    explicit Quadric3D(const MatrixInitType& i)
      : Matrix4x4<QUADRIC3D_TYPE>(i){};

    Quadric3D(const Matrix<QUADRIC3D_TYPE> &M) { (*this) = M; };
        
    /** @brief constructs an ellipsoid around the center C

        The covariance matrix's stddevs along the main axes (multiplied by
        dSize) are used  as the ellipsoid's radii 
        for dsize==1 this is the standard confidence region (SCR)
        see Kanatani: "Statistical optimization ...", p.122
        The probability that the point is in the SCR is about 20% 
        See doc above for values for other probabilities */
    Quadric3D(const HomgPoint3D& C, const CovMatrix3x3& cov, 
              const double& dScale = GAUSS3D_CONFIDENCE_20_PERCENT);

    /** @brief determines if point3D is inside/on/outside the surface

        For a normalized ellipsoid, 
        a negative return value means the point is inside, a positive value
        means outside and 0 means exactly on the surface (you 
        have to use epsilon environments for all tests)
        LocatePoint internally computes the difference of Mahalanobis distances
        of point3D from the center and a surface point from the center */
    inline double LocatePoint(const HomgPoint3D& point3D) const {
      return ( point3D *((*this)*point3D) ); }
      
    /** @brief sets trace of upperleft submatrix3x3 to unity */
    inline bool Normalize() {
      double d = (*this)[0][0]+(*this)[1][1]+(*this)[2][2];
      if (fabs(d)<DBL_EPSILON) return false;
      (*this) /= d; return true; }

    Quadric3D& operator=(const Matrix<QUADRIC3D_TYPE> &M) {
      Matrix4x4<QUADRIC3D_TYPE>::operator=(M); return (*this);
    }

    /** @brief computes the dual quadric (points <-> planes) 
        @param UseSVD use numerically robust svd or fast analytic inverse
        @author koeser 03/2005 */
    Quadric3D GetDualQuadric(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();

  };
  
  
}

#endif