ThreeDOut.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 _BIASTHREEDOUT_HH_
#define _BIASTHREEDOUT_HH_

#include <bias_config.h>

#include <cmath>
#include <list>
#include <vector>
#include <map>
// BIAS
#include <Base/Debug/Debug.hh>
#include <Base/Image/Image.hh>
#include <Base/Math/Vector3.hh>
#include <Base/Math/Vector4.hh>
#include <Base/Math/RGBA.hh>
#include <Base/Geometry/HomgPlane3D.hh>
#include <Base/Geometry/HomgPoint3D.hh>
#include <Geometry/PMatrix.hh>
#include <Geometry/Quadric3D.hh>
#include <Geometry/CovMatrix3x3.hh>
#include <Utils/PMatrix3DData.hh>
#include <Utils/TriangleMesh.hh>
#include <Geometry/Projection.hh>
#include <Geometry/ProjectionParametersBase.hh>
#include <Base/Geometry/PoseParametrization.hh>

#define GL_GLEXT_PROTOTYPES

#ifdef BIAS_HAVE_OPENGL
#  include <Gui/biasgl.h>
#ifdef __APPLE__
# include <OpenGL/glu.h>
#else // __APPLE__
# include <GL/glu.h> 
#endif // __APPLE__
//#  include <GL/gl.h>
  //#  include <GL/glx.h>
#endif

/// use one sigma as the standard confidence region for ellipsoid surfaces
/// if you want to supply a probability, see defines in Quadric3D.hh
#define DEF_P_SCALE 1.0

// opaque, non transparent surface alpha value
#define ALPHA_TRANSPARENT 0
#define ALPHA_OPAQUE      255

/// entity index offsets for selection rendering/picking. (OpenGL GL_SELECT)
/// Each entity index is mapped to an int using a entity specific offset
/// Thus the nr. of entites is currently limited to 100.000 !
/// Assert unique indexing, ranges must not overlap. 
/// @author Jan Woetzel
//#define BIAS_SELECTION_OFFSET_IMAGE      100000
#define BIAS_SELECTION_OFFSET_PMATRIX    200000
//#define BIAS_SELECTION_OFFSET_LINE       300000
//#define BIAS_SELECTION_OFFSET_ELLIPSOID  400000
//#define BIAS_SELECTION_OFFSET_POINT      500000

// BIASDOUT - log destination (?)
#define D_3DOUT_PDECOMP 0x00001000

namespace BIAS {

  /** \class Projection3DData
   *  \brief stores data for drawing a the extrinisc parameters of a projection.
   */
  class BIASUtils_EXPORT Projection3DData {
  public:
    inline void Set(const BIAS::PoseParametrization& p, 
                    const double scale=1.0) {
      

      C = p.GetPosition();
      Quaternion<double> Q = p.GetOrientation();
      RMatrix R;
      R.SetFromQuaternion(Q);

      Set(C, R, scale);
    }
    /**
       @bug this only works for projections with only one camera defined
       @bug extend it to iterate over all cameras ore use 
            Set(const BIAS::ProjectionParametersBase& p,...)
    */
    inline void Set(const BIAS::Projection& p, const double scale=1.0 ) {
      unsigned int width, height;
      p.GetParameters()->GetImageSize(width, height);
      
      ratio = (double)width/(double)height;

      C = p.GetC();
      RMatrix R = p.GetR();
      Set(C, R, scale);         
      identifier = p.GetIdentifier();
    }
    
    /**
       @brief Only use this if you have a single camera or a Projection
              with absolute Transformations of your cameras
              It does not handle relative Transformations between first
              and successive cameras.              
    */
    inline void Set(const BIAS::ProjectionParametersBase& p,
                    const double scale=1.0 ) {
      unsigned int width, height;
      p.GetImageSize(width, height);      
      ratio = (double)width/(double)height;

      C = p.GetC();
      RMatrix R = p.GetR();
      Set(C, R, scale);          
    }

    

    inline void Set(const BIAS::Vector3<double>& C,
                    const BIAS::RMatrixBase& R,
                    const double scale=1.0) {
      for(unsigned int i=0; i<3; i++) {
        H[i] = scale*R[i][0];
        V[i] = scale*R[i][1];
        A[i] = scale*R[i][2];
      }

      H = H+C;
      V = V+C;
      A = A+C;

      R.GetRotationAxisAngle(axis, angle);
    }
    
    Projection3DData() : radiusSelection(0.1) {}
    Vector3<double> C;
    Vector3<double> A;
    Vector3<double> H;
    Vector3<double> V;
    Vector3<double> axis;
    double angle;
    
    double ratio;

    double radiusSelection;

    //type: 0 -    perspective camera
    //      1 - nonperspective camera   
    //     -1 - no type of camera selected
    int    type;
 
    double scale;

    BIAS::RGBAuc ColorOfSelection;
    std::string identifier;
  };

  /** @class Ellipsoid3DData
   *  @brief stores important data for drawing an el4000lipsoid
   */
  class BIASUtils_EXPORT Ellipsoid3DData {
  public:
    BIAS::Vector3<double> RotAxis; ///< rot to get 1st mainaxis from (1,0,0)
    double dAngle;                 ///< rotation in radians around RotAxes 
    double s0, s1, s2;             ///< lengths of main axes
    BIAS::Vector3<double> C;       ///< center of ellipsoid
    BIAS::RGBAuc Color;
  };

  /** @class Text3DData
   *  @brief stores important data for drawing 3d text
   */
  class BIASUtils_EXPORT Text3DData {
  public:
    BIAS::Vector3<double> Position;
    BIAS::Quaternion<double> Direction;
    std::string Text;
    double Size;
    BIAS::RGBAuc Color;
  };


  /** @class Point3DData
      @brief stores data for drawing a point */
  class BIASUtils_EXPORT Point3DData {
  public:
    BIAS::Vector3<double> Point;
    BIAS::RGBAuc Color;
  };

  /** @class Image3DData
   *  @brief  stores datas for a textured plane
   */
  class BIASUtils_EXPORT Image3DData {
  public:
    Vector3<double>  UL;       ///< upper left corner of image plane in wcs
    Vector3<double>  UR;       ///< upper right corner of image plane in wcs
    Vector3<double>  LL;       ///< lower left corner of image plane in wcs
    Vector3<double>  LR;       ///< lower right corner of image plane in wcs
    Image<unsigned char> ImageData;      ///< data of texture image
    bool BillBoard;
    std::string name;
    Image3DData() : BillBoard(false) {}

    Image3DData(const Image3DData& d){
      UL=d.UL; UR=d.UR; LL=d.LL; LR=d.LR; ImageData=d.ImageData; 
      name=d.name;  BillBoard = d.BillBoard;
    }

  };

  /** @class Line3DData
   *  @brief stores data for drawing a line
   */
  class BIASUtils_EXPORT Line3DData {
  public:
    BIAS::Vector3<double> Start;
    BIAS::Vector3<double> End;
    BIAS::RGBAuc Color;
  };

  /** @class IndexedFaceSet
   *  @brief stores data for drawing a indexed face set.
   *   Faces beeing (preferably flat) polygons
   */
  class BIASUtils_EXPORT IndexedFaceSet {
  public:

    IndexedFaceSet();
    IndexedFaceSet(const IndexedFaceSet& a);
    ~IndexedFaceSet();

    BIAS::Image<unsigned char> texture;
    std::vector<BIAS::Vector3<double> > vertexSet;
    std::vector<BIAS::Vector4<unsigned char> > vertexColorSet;
    std::vector<BIAS::Vector2<float> > texCoordSet;
    //each BIAS::Vector storing a face 
    std::vector<BIAS::Vector<int> > faceIndices; 
    std::string name;
    std::string textureName;
    bool writeOutTexture;
    /// bounding box
    Vector3<double> bbmin, bbmax;
    bool useTexture;
    bool useColoredVertices;
    bool useNormals;

    int VASize;
    int CASize;
#ifdef BIAS_HAVE_OPENGL
    int TASize, NumFacets;
    bool compiled_;
    GLuint displayList_;
    GLuint textureObjectID_;
    GLfloat *VertArray, *NormalArray;
    GLfloat *TextureCoordArray;
    GLubyte *ColorArray;
    // array holding pointer to indices making up a facet
    // facet[0] -> {0, 100 , 200}
    GLuint** facetArray;
    // number of indices a single facet is using
    //facetIndicesCountArray[0] = 3
    GLsizei* facetIndicesCountArray;
#else
    float *NormalArray;
#endif
    
  };

  /** @enum CameraDrawingStyle
      @brief different styles when drawing cameras, e.g. in GL or VRML */
  enum CameraDrawingStyle { PyramidMesh, PyramidSolid, Arrow, 
                            PyramidMeshWithArrow, PyramidWithCenterBox, Undrawn };

  /** @enum PointDrawingStyle
      @brief different styles when drawing points, e.g. in GL or VRML */
  enum PointDrawingStyle { Point, PointSprite, Sphere, Box};

  /** @enum PointDrawingStyle
      @brief different styles when drawing lines, e.g. in GL or VRML */
  enum LineDrawingStyle { Solid, Dashed };

  /** @struct ThreeDOutParameters
   *  @brief configuration struct for drawing styles of various 3d objects
   *  @ingroup g_utils
   */
  class BIASUtils_EXPORT ThreeDOutParameters {
  public:
    ThreeDOutParameters();
    PointDrawingStyle PointStyle;
    CameraDrawingStyle CameraStyle;
    LineDrawingStyle LineStyle;
    bool DrawUncertaintyEllipsoids;
    bool DrawCameraUncertaintyEllipsoids;
    double PointSize;
    double CameraScale;
    int LineWidth; ///< in pixel
    double EllipsoidScale;
    double WCSAxesLength; ///< <=0 means no visual coordinate axes
#ifdef BIAS_HAVE_OPENGL
    GLenum IndexedFaceSetPrimitiveMode;
#endif
  };

  /** @class ThreeDOut
    * @ingroup g_utils
      @brief Unified output of 3D entities via OpenGL or VRML.

            Alpha blending policy (like OpenGL defaults):
      - Alpha blending value means opacity (not transparency), e.g.
      0          = full transparent (invisible glass), see ALPHA_TRANSPARENT
      255 (1.0f) = opaque (not transparent), see ALPHA_OPAQUE
      - Thus set OpenGL state of calling application accordingly e.g.
      \verbatim
      glEnable   (GL_BLEND);
      glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
      \endverbatim
      - Remember to render in correct order:  
      -  opaque primitives first, 
      -  back to front order.
      Details: 
      http://www.opengl.org/resources/faq/technical/transparency.htm 

      However, VRML has inverse interpretation (!), 
      thus a conversion is required, see 
      http://www.web3d.org/x3d/specifications/vrml/ISO-IEC-14772-VRML97/part1/nodesRef.html  */
  class BIASUtils_EXPORT ThreeDOut : public Debug 
  {
  public:

    // @brief Flag indicating whether the viewports should be displayed
    //        with cones and pyramids or not
    bool visualize_viewports;

    /// @brief Destructor releases all memory used
    virtual ~ThreeDOut();

    /// @brief Constructor for empty 3D scene with given parameters
    ThreeDOut(const BIAS::ThreeDOutParameters& params =
              BIAS::ThreeDOutParameters());

    void SetParamsPointSize(const double pointSize);
    void SetParamsLineWidth(const int lineWidth);
    void SetParamsCameraScale(double cameraScale); // only works for OpenGLOut!
    void SetParamsCameraStyle(CameraDrawingStyle cameraStyle);
    void SetParamsDrawEllipse(bool drawEllipse);
    void SetParamsDrawCameraEllipse(bool drawEllipse);
    void SetParamsEllipsoidScale(double ellipsoidScale);
    void SetParamsWCSAxesLength(double WCSAxesLength);
    void SetParamsPointDrawingStyle(const PointDrawingStyle pointStyle);
    void SetParamsLineDrawingStyle(const LineDrawingStyle lineStyle);

    void SetParams(const ThreeDOutParameters& params);

    /** @brief transfers the whole scene by this offset */
    void Translation(const Vector3<double>& movement);

    /** @name Functions to add geometric objects for output
        @brief save the return value to update or delete the objects later 
        @{ */

    /** @return index value to change the point later
        @param v point coordinates */
    unsigned int AddPoint(const BIAS::Vector3<double> &v,
                          const BIAS::RGBAuc &Color=RGBAuc_WHITE_OPAQUE );

    /** @return index value to change the text later
        @param position text coordinates
        @param orientation 3d orientation (screen-aligned if zero)
        @param thetext string to be visualized in 3d
        @param thesize: if>0: 3d height of text
                        if<0: 2d height (in pixels) of text */
    unsigned int AddText(const std::string& thetext,
                         const BIAS::Vector3<double> &position,
                         const BIAS::Quaternion<double> &direction=
                           BIAS::Quaternion<double>(0.0,0.0,0.0,0.0),
                         const double& thesize = -12.0,
                         const BIAS::RGBAuc &Color=RGBAuc_WHITE_OPAQUE );


    /** @return index value to change the point later
        @param v point coordinates */
    unsigned int AddPoint(const BIAS::HomgPoint3D &v, 
                          const BIAS::RGBAuc &Color=RGBAuc_WHITE_OPAQUE );

    /** @return index value to change the line later
        @param Start Start point of line
        @param End End point of line */
    unsigned int AddLine(const BIAS::Vector3<double> &Start,
                         const BIAS::Vector3<double> &End,
                         const BIAS::RGBAuc &Color=RGBAuc_WHITE_OPAQUE );

    /** @return index value to change the line later
        @param Start Start point of line
        @param End End point of line */
    unsigned int AddLine(const BIAS::HomgPoint3D &Start,
                         const BIAS::HomgPoint3D &End, 
                         const BIAS::RGBAuc &Color=RGBAuc_WHITE_OPAQUE);

    /** @param dConfidenceRegionScale see Quadric3D.hh for the relationship
        between enlarging the standard confidence region and the probability
        that a measurement will be inside that region, 1.0 means sigma */
    unsigned int AddEllipsoid(BIAS::CovMatrix3x3 &cov, 
                              const BIAS::Vector3<double> &C,
                              const BIAS::RGBAuc &Color=RGBAuc_WHITE_SEMI,
                              const double& dConfidenceRegionScale = DEF_P_SCALE, 
                              const bool cameraCov = false);

    /** @param dConfidenceRegionScale see Quadric3D.hh for the relationship
        between enlarging the standard confidence region and the probability
        that a measurement will be inside that region, 1.0 means sigma */
    unsigned int AddEllipsoid(BIAS::CovMatrix3x3 &cov, 
                              const BIAS::HomgPoint3D &C,
                              const BIAS::RGBAuc &Color=RGBAuc_WHITE_SEMI,
                              const double& dConfidenceRegionScale = DEF_P_SCALE, 
                              const bool cameraCov = false);

    /**
     * @brief Adds the given plane to the scene, visualising it with lines
     *        or with a colored quad.
     *
     * @note The translation vector may render the plane's distance to the
     *       center of origin inconsistent. You must compute a proper
     *       translation vector that is located on the plane by yourself, e.g.
     *       by using HomgPlane3D::ProjectPoint().
     *
     * @param plane
     *        the plane to add
     * @param colour
     *        the colour used for drawing
     * @param size
     *        edge length of the plane
     * @param translation
     *        the lines are moved by this vector
     * @param wireframe
     *        specifies if plane should be displayed with lines or solid
     *
     * @author rwulff
     * @date   09/2011
     */
    void AddPlane(const HomgPlane3D& plane,
                  const RGBAuc& colour = RGBAuc_WHITE_SEMI,
                  const double size = 1.0,
                  const Vector3<double>& translation = Vector3<double>(0.0, 0.0, 0.0),
                  const bool wireframe = true);

    /**
     * @brief Helper function computing the corners of a 3D plane.
     *
     * @param plane
     *        the plane to compute corners for
     * @param size
     *        edge length of the plane
     * @param corners
     *        returns 3D coordinates of plane corners
     *
     * @author esquivel
     * @date   06/2012
     */
    static void ComputePlaneCorners(const HomgPlane3D& plane,
                                    const double size,
                                    std::vector< Vector3<double> >& corners);

    /** decompose P and add to internal data structures.
        \param name defines node name in vrml - does not like dots ('.')!
        @todo the scale of the camera should be a (dynamic) visualization 
        option, not a parameter of the add function
        @author woelk 01/2004 */
    unsigned int AddPMatrix(BIAS::PMatrix &P,
                            const unsigned int& width,
                            const unsigned int& height,
                            const BIAS::RGBAuc &Color = RGBAuc_WHITE_OPAQUE,
                            const double &dScale = DEF_P_SCALE,
                            const std::string& name = "");

    /** interface for the above @author Jan Woetzel */
    unsigned int AddPMatrix(BIAS::Matrix3x4<double> &M, 
                            const unsigned int& width,
                            const unsigned int& height,
                            const BIAS::RGBAuc &Color = RGBAuc_WHITE_OPAQUE,
                            const double &dScale = DEF_P_SCALE,
                            const std::string& name = "");


    /** @brief add a small rectangular image patch spanned by three 3d points
        small means really small (only a few pixels), the patch is approximated
        in a very complicated way in vrml to allow for inline data
        @param channels number of channels: 1=grey, 3=rgb
        @author koeser 09/2004 */
    inline unsigned int AddImage(const Vector3<double>& UL,
                                 const Vector3<double>& UR,
                                 const Vector3<double>& LL,
                                 unsigned int Width, unsigned int Height,
                                 unsigned char* pData,
                                 unsigned int channels = 1,
                                 const std::string& name = "",
                                 bool billboard = false) 
    {
      Vector3<double> LR = LL + (UR-UL);
      return AddImage(UL, UR, LL, LR, Width, Height, pData, channels, 
                      name, billboard);
    }

    /** @brief add a small image patch spanned by four 3d points
        small means really small (only a few pixels), the patch is approximated
        in a very complicated way in vrml to allow for inline data
        @param channels number of channels: 1=grey, 3=rgb
        @author koeser 09/2005 */
    unsigned int AddImage(const Vector3<double>& UL,
                          const Vector3<double>& UR,
                          const Vector3<double>& LL,
                          const Vector3<double>& LR,
                          unsigned int Width, unsigned int Height,
                          unsigned char* pData,
                          unsigned int channels = 1,
                          const std::string& name = "",
                          bool billboard=false);


    /** Adds triangle mesh as IndexedFaceSet to ThreeDOut mem.
     * \param name defines node name in vrml - must not contain dots ('.')!
     * \param textureOutputName is telling which name the texture shall have
     *        when writing the vrml. If this param is empty (default), the texture is
     *        integrated into the vrml as PixelMap.
     * \param writeOutTexture if true and the textureOutputName is not "" then
     *        the texture is written into textureOutputName, otherwise only the texture
     *        name or a PixelMap is integrated into vrml. (default = true)
     * \param calcNormals[in]: set whether to calculate normals (e.g. for lighting) or not.
     *        Normally normals are only calculated if no texture is present in mesh (default behaviour)
    */
    unsigned int AddTriangleMesh(const TriangleMesh& mesh, 
                                 const std::string& name="",
                                 const std::string& textureOutputName = "",
                                 bool writeOutTexture = true,
                                 bool calcNormals = false);

    /** Add a projection object to ThreeDOut mem.
     * \param colorSelection color of the small sphere around the center
     * Draws the four cam coordinate axes (A in red, H in Green, V in blue)
     * and a small sphere around the center which is usefull for selection.
     */
    unsigned int AddProjection(const Projection& p, 
                               const RGBAuc& colorSelection = 
                               RGBAuc_WHITE_OPAQUE, 
                               const double radius = 0.1,
                               const double scale = 1.0,
                               const std::string& identifier = "",
                               const int    camera_type = -1);
      
    /// interface for above
    unsigned int AddProjection(const Projection& p, 
                               const std::string& identifier)
    {
      return AddProjection(p, 
                           RGBAuc_WHITE_OPAQUE, 
                           0.1,
                           1.0,
                           identifier,
                           -1);
    };

    /** @author bartczak */
    unsigned int AddPose(const PoseParametrization& p, 
                         const RGBAuc& colorSelection = RGBAuc_WHITE_OPAQUE, 
                         const double radius = 0.1,
                         const double scale = 1.0,
                         const int    camera_type = -1);

    /** @} 
        @name Functions to update previously added objects for output
        @{ */

    /** @param PointIndex value previously returned by AddPoint */
    void UpdatePoint(const unsigned int& PointIndex,
                     const BIAS::Vector3<double> &v,
                     const BIAS::RGBAuc &Color = RGBAuc_WHITE_OPAQUE);

    /** @param PointIndex value previously returned by AddPoint */
    void UpdatePointColor(const unsigned int& PointIndex,
                          const BIAS::RGBAuc &Color);

    /** @param PointIndex value previously returned by AddPoint */
    void UpdatePoint(const unsigned int& PointIndex,
                     const BIAS::HomgPoint3D &v,
                     const BIAS::RGBAuc &Color = RGBAuc_WHITE_OPAQUE);

    /** @param PIndex value previously returned by AddPMatrix */
    void UpdatePMatrix(const unsigned int& PIndex, BIAS::PMatrix &P, 
                       const unsigned int& width,
                       const unsigned int& height,
                       const BIAS::RGBAuc &Color = RGBAuc_WHITE_OPAQUE,
                       const double& dScale = DEF_P_SCALE);

    /** @param PIndex value previously returned by AddPMatrix */
    void UpdatePMatrixColor(const unsigned int& PIndex, 
                            const BIAS::RGBAuc &Color);

    /** @param LineIndex value previously returned by AddLine
        @param Start Start point of line
        @param End End point of line */
    void UpdateLine(const unsigned int& LineIndex,
                    const BIAS::Vector3<double> &Start,
                    const BIAS::Vector3<double> &End,
                    const BIAS::RGBAuc &Color = RGBAuc_WHITE_OPAQUE);

    /** @param LineIndex value previously returned by AddLine
        @param Start Start point of line
        @param End End point of line */
    void UpdateLine(const unsigned int& LineIndex,
                    const BIAS::HomgPoint3D &Start,
                    const BIAS::HomgPoint3D &End, 
                    const BIAS::RGBAuc &Color = RGBAuc_WHITE_OPAQUE);

    /** @param dConfidenceRegionScale see Quadric3D.hh for the relationship
        between enlarging the standard confidence region and the probability
        that a measurement will be inside that region, 1.0 means sigma */
    void UpdateEllipsoid(const unsigned int& EIndex,
                         BIAS::CovMatrix3x3 &cov, 
                         BIAS::Vector3<double> &C,
                         const BIAS::RGBAuc &Color = RGBAuc_WHITE_SEMI,
                         const double& dConfidenceRegionScale = DEF_P_SCALE);

    /** @param dConfidenceRegionScale see Quadric3D.hh for the relationship
        between enlarging the standard confidence region and the probability
        that a measurement will be inside that region, 1.0 means sigma */
    void UpdateEllipsoid(const unsigned int& EIndex,
                         BIAS::CovMatrix3x3 &cov, 
                         BIAS::HomgPoint3D &C,
                         const BIAS::RGBAuc &Color = RGBAuc_WHITE_SEMI,
                         const double& dConfidenceRegionScale = DEF_P_SCALE);
    /** @} 
        @name Functions to remove previously added objects for output      
        @{ */

    /** @brief removes all entites
        Essentially a clear on each entityt container.
        @author Jan Woetzel */
    void RemoveAll();

    /** @brief completely removes the point from the internal structures,
        there is no way to get it back */
    void RemovePoint(const unsigned int& PointIndex);

    /** @brief completely removes all points from the internal structures,
        there is no way to get them back */
    void RemoveAllPoints();

    /** @brief completely removes the line from the internal structures,
        there is no way to get it back */
    void RemoveLine(const unsigned int& LineIndex);

    /** @brief completely removes all lines from the internal structures JW */
    void RemoveAllLines();

    /** @brief completely removes the ellipsoid from the internal structures,
        there is no way to get it back */
    void RemoveEllipsoid(const unsigned int& EIndex);

    /** @brief completely removes all ellipsoids from the internal structures,
        there is no way to get it back */
    void RemoveAllEllipsoids();

    void RemoveAllCamEllipsoids();

    /** @brief completely removes the image from the internal structures,
        there is no way to get it back */
    void RemoveImage(const unsigned int& EIndex);

    /** @brief completely removes all images from the internal structures,
        there is no way to get it back */
    void RemoveAllImages();

    void RemoveAllIndexedFaceSets();

    /** @brief removes the P-matrix index,
        there is no way to get it back */
    void RemovePMatrix(int index);

    /** @brief completely removes all images from the internal structures,
        there is no way to get it back */
    void RemoveAllPMatrices();

    /** @} 
        @name Functions for openGL output (if BIAS_HAVE_OPENGL)
        @{ */

#ifdef BIAS_HAVE_OPENGL

    /// @brief draw all points with the same color:
    void OpenGLOutPoints(const BIAS::RGBAuc &Color);

    /// @brief draw all points with their own color:
    void OpenGLOutPoints();

    /// @brief draw all pmatrices with the same color:
    void OpenGLOutPMatrices(const BIAS::RGBAuc &Color);

    /// @brief draw all pmatrices with their own color:
    void OpenGLOutPMatrices();

    /// @brief draw all lines with the same color:
    void OpenGLOutLines(const BIAS::RGBAuc &Color);

    /// @brief draw all pmatrices with their own color
    /// @todo linedrawingstyle=dashed not implmented
    void OpenGLOutLines();

    /// @brief draw all ellipsoids with the same color:
    void OpenGLOutEllipsoids(const BIAS::RGBAuc &Color);

    /// @brief draw all ellipsoids with their own color:
    void OpenGLOutEllipsoids();

    /// @brief draw mesh into opengl
    void OpenGLOutIndexedFaceSets();

    /** @brief draw all image patches using OpenGL
        @param disableBlending true to glDisbale(GL_BLEND) temporarily
        \todo: This implementation is very inefficient because it 
        recreates the textures for each rendering pass! JW  */
    void OpenGLOutImages(const bool & disableBlending=true);

    /** @brief display the world coordinate system origin and axes */
    void OpenGLOutWCS();

    /** @brief draw all entities (with the same color) using OpenGL
        Includes lines, points, pmatrices etc. (everything) */
    void OpenGLOut(const BIAS::RGBAuc &Color);

    /** @brief render all entities (with the same color) using OpenGL
        Includes lines, points, pmatrices etc. (everything) */
    void OpenGLOut();


#endif // BIAS_HAVE_OPENGL

    /// get center of gravity (mean of structure)
    Vector3<double> GetCenterOfPoints();

    /** Get the min and max vector of the smallest bounding box, which
     * is drawn around the added points.
     * \brief uncompleted, doesn't deliver correct results (mostly 0)
     * \status alpha
     */
    void GetBoundingBox(Vector3<double> &min,Vector3<double> &max);

    /** @} 
        @name Functions for VRML output
        @{ */

    /** @brief Generic write method (wrapper for VRMLOut). */
    virtual int WriteToFile(const std::string &filename) {
      return VRMLOut(filename);
    }

    /** @brief flush all 3d objects to a vrml file with name sFilename, this 
        is the function most users would call */
    int VRMLOut(const std::string &sFilename);

    /** @brief flush all 3d objects to a vrml file with name sFilename, use
        Color as the color for all elements */
    int VRMLOut(const std::string &sFilename, const BIAS::RGBAuc &Color);

    /** @brief only write (previously stored) points into an open vrml file,
        override their individual colors with Color */
    int VRMLOutPoints(std::ostream& vrml, const BIAS::RGBAuc &Color);

    /** @brief only write (previously stored) points into an open vrml file */
    int VRMLOutPoints(std::ostream& vrml);

    /** @brief only write (previously stored) cameras into an open vrml file,
        override their individual colors with Color */
    int VRMLOutPMatrices(std::ostream& vrml, const BIAS::RGBAuc &Color);


    /** @brief only write (previously stored) cameras into an open vrml file */
    int VRMLOutPMatrices(std::ostream& vrml);

    /** @brief only write camera into an open vrml file */
    int VRMLOutPMatricesRoutine(std::ostream& VRMLFile,
                                const PMatrix3DData *it,
                                double r=255.0,
                                double g=255.0,
                                double b=255.0,
                                double transparency=0.0,
                                bool CenterAsSphere=false);

    /** @brief only write (previously stored) lines into an open vrml file,
        override their individual colors with Color */
    int VRMLOutLines(std::ostream& vrml, const BIAS::RGBAuc &Color);

    /** @brief only write (previously stored) points into an open vrml file */
    int VRMLOutLines(std::ostream& vrml);

    /** @brief only write (previously stored) ellipsoids to an open vrml file,
        override their individual colors with Color */
    int VRMLOutEllipsoids(std::ostream& vrml, const BIAS::RGBAuc &Color);

    /** @brief only write (previously stored) ellipsoids into an open vrml 
        file */
    int VRMLOutEllipsoids(std::ostream& vrml);

    /** @brief only write (previously stored) images into an open vrml file */
    int VRMLOutImages(std::ostream& VRMLFile);

    /** @brief only write (previously stored) face sets into an open vrml file 
     * Will include texture into VRML as PixelTexture node.
     */
    int VRMLOutIndexedFaceSets(std::ostream& VRMLFile);

    /** only write previously stored projection objects into opened vrml file.
     */
    int VRMLOutProjection(std::ostream& VRMLFile);

    /** only write previously stored projectionview objects into opened vrml 
     *  file.
     */
    int VRMLOutProjectionView(std::ostream& VRMLFile);

    /** @brief write the VRML 2.0 header into an open file */
    int VRMLOutWriteHeader(std::ostream& vrml);

    /** @brief write VRML 2.0 navigation info node @author Jan Woetzel */
    void VRMLOutWriteNavigationInfo(std::ostream& vrml);

    /** write VRMl 2.0 viewpoint that can selecetd in vrml viewer 
        @author Jan Woetzel */
    void VRMLOutWriteViewpoint(std::ostream& vrml);

    /** @brief write world coordinate system origin and axes */
    void VRMLOutWCS(std::ostream& vrml);

    /** @} 
        @name Debug functions
        @{ */
    /** @brief list all internal 3d objects on cout */
    void Dump();
    /** @}  */

    /** call by reference accessor for data member mapPMatrices_  JW */
    inline std::map< unsigned int, PMatrix3DData > & GetMapPMatrices() 
    {
      return mapPMatrices_;
    }

    /** get alpha opacity value as VRML transparency 
        helper mapping, usually transparency = (1-alpha) */
    static double VRMLtransparency(const unsigned char alpha);

    /** get alpha opacity component as VRML transparency g */
    static double VRMLtransparency(const BIAS::Vector4<unsigned char> & vRgba);

    /** @brief write VRML viewpoint nodes for some P matrices ? 
        @author Jan Woetzel */
    void SetWriteViewpoints(const bool & val);

    // ------------------------------------------------------------
  protected:

    void DrawSphere_(double radius, int lats, int longs);

    //TWO MAIN METHODS OF DESCRIBING PYRAMID AND PAINTCONE IN VRML

    /** @brief Write a parameterized pyramid into VRMLFile.
     * @param VRMLFile destination, writes to an end of the file
     * @param selectionRadius length of an edge of the pyramid
     * @param transparency transparency of given (r,g,b)-color
     * @param angle angle between two opposite faces of pyramid which 3rd 
     *        edges are width-of-the-projection far from eachother
     * @param ratio width-of-the-projection/height-of-the-projection
     * @param paint_lines redraw 4 edges of the pyramid (looks better)
     * @author amattal 
     */
    void PaintPyramid(std::ostream& VRMLFile,
                      double selectionRadius,
                      double r, double g, double b, double transparency,
                      double angle,
                      double ratio = 4.0/3.0,
                      double scale = 1.0,
                      const std::string& name = "",
                      bool paint_lines = false);

    /** @brief Write a parameterized cone into VRMLFile.
     * @param VRMLFile destination, writes to an end of the file
     * @param selectionRadius length of the side of the cone
     * @param transparency transparency of given (r,g,b)-color
     * @param angle angle of the cone
     * @author amattal 
     */
    void PaintCone(std::ostream& VRMLFile,
                   double selectionRadius,
                   double r, double g, double b, double transparency,
                   double angle,
                   double scale = 1.0,
                   const std::string& name = "");
    //------------------------------------------------------------

#ifdef BIAS_HAVE_OPENGL
    void OpenGLOutPMatrices_(const BIAS::RGBAuc &Color, bool UseOneColor);
#endif
#ifdef BIAS_HAVE_GLEW
    bool glewInitialized_;
#endif
    
    //THREE MAIN METHODS OF GRAPHICALLY DESCRIBING VIEWPORTS IN VRML
    
    /** @brief Write a projection of a PERSPECTIVE camera into VRML.
     * @param VRMLFile destination, writes to an end of the file
     * @param selectionRadius length of an edge of the pyramid(s)
     * @param angle maximum angle of the camera-view
     * @param ratio width-of-the-projection/height-of-the-projection
     * @param scale scale of the projection
     * @param solid true if Style = PyramidSolid 
     * @author amattal 
     */
    void VRMLOutViewPortPyramidal(std::ostream& VRMLFile,
                                  double selectionRadius,
                                  double r, double g, double b,
                                  double angle,
                                  double ratio,
                                  double scale,
                                  const std::string& name,
                                  bool solid = false);

    /** @brief Write a projection of a NON-PERSPECTIVE camera into VRML.
     * @param VRMLFile destination, writes to an end of the file
     * @param selectionRadius length of a side of the cone(s)
     * @param angle maximum angle of the camera-view
     * @param scale scale of the projection
     * @param solid true if Style = PyramidSolid 
     * @author amattal 
     */
    void VRMLOutViewPortConical(std::ostream& VRMLFile,
                                double selectionRadius,
                                double r, double g, double b,
                                double angle,
                                double scale,
                                const std::string& name,
                                bool solid = false);

    /** @brief Write a projection of a camera in COORDINATE-style into VRML.
     * @param VRMLFile destination, writes to an end of the file
     * @param scale scale of the projection
     */
    void CoordinateSystem(std::ostream& VRMLFile,
                          double selectionRadius,
                          double r, double g, double b, double axeslength,
                          double scale = 1.0,
                          const std::string& name = ""); 
    //------------------------------------------------------------
    
    // helper for VRMLOutPoints()
    int VRMLOutPointsAsPointSet_(std::ostream &VRMLFile);
    int VRMLOutPointsAsBoxes_(std::ostream &VRMLFile);
    int VRMLOutPointsAsSpheres_(std::ostream &VRMLFile);

    /// this is an optimized strategy to avoid hundred of shape nodes
    int VRMLOutPMatricesAsArrows_(std::ostream& vrml, const BIAS::RGBAuc &Color);
    int VRMLOutPMatricesAsMesh_(std::ostream& vrml, const BIAS::RGBAuc &Color);

    void VRMLOutEllipsoid_(std::ostream& vrml, Ellipsoid3DData& E, 
                           const BIAS::RGBAuc &Color);

    /** @brief replace invalid characters in def string and return corrected
        @author koeser */
    std::string CorrectDefString_(const std::string& defstring);

    /// holds the params set by constructor or SetParams
    ThreeDOutParameters Params_;

    /// the cameras are saved in a vector for efficiency reasons
    std::map<unsigned int, PMatrix3DData > mapPMatrices_;
    /// counter for the next index, gaps (due to delete) are not reused by now
    unsigned int NextPIndex_;

    /// map of all image structures
    std::map<unsigned int, Image3DData> mapImages_;
    /// counter for the next index, gaps (due to delete) are not reused by now
    unsigned int NextImageIndex_;

    /// map of all point structures
    std::map<unsigned int, Point3DData> mapPoints_;
    /// counter for the next index, gaps (due to delete) are not reused by now
    unsigned int NextPointIndex_;

    /// map of all point structures
    std::map<unsigned int, Text3DData> mapTexts_;
    /// counter for the next index, gaps (due to delete) are not reused by now
    unsigned int NextTextIndex_;

    /// map of all line structures
    std::map<unsigned int, Line3DData> mapLines_;
    /// counter for the next line index, gaps (due to delete) arent reused
    unsigned int NextLineIndex_;

    /// map of all ellipsoid structures
    std::map<unsigned int, Ellipsoid3DData> mapEllipsoids_;
    /// counter for the next ellipsoid index, gaps are not reused by now
    unsigned int NextEllipsoidIndex_;

    /// map of all ellipsoid structures
    std::map<unsigned int, Ellipsoid3DData> mapCamEllipsoids_;
    /// counter for the next ellipsoid index, gaps are not reused by now
    unsigned int NextCamEllipsoidIndex_;

    std::map<unsigned int, IndexedFaceSet> mapIndexedFaceSet_;
    unsigned int NextIndexedFaceSetIndex_;

    std::map<unsigned int, Projection3DData> mapProjection3D_;
    unsigned int NextProjection3DIndex_;

    bool WriteViewpoints_; //< write VRML viewpoint nodes for (some) PMatrices

    std::vector<double> meanDraw;

  }; // end class

} // end namespace

#endif