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

#ifdef WIN32
#  pragma warning (disable: 4251) // STL MAP DLL warning noise (JW)
#endif

#include <ctype.h>
#include "ThreeDOut.hh"
#include <Base/Debug/TimeMeasure.hh>
#include <Base/Image/ImageIO.hh>
#include <Geometry/RMatrix.hh>

#if defined(BIAS_HAVE_OPENGL) && defined(BIAS_HAVE_FONTGL)
#include <Utils/DrawTextGL.hh>
#endif

#ifdef BIAS_HAVE_OPENGL
#  include <Utils/StatekeeperGL.hh>
#endif

#define ASSERT_MAP_ELEMENT(themap, key) if (themap.find(key)==themap.end()) { \
  BIASERR("Tried to access map with key " \
  <<key<<". Undefined Element." \
  <<" Aborting."); BIASABORT; }

#define DEGREE_PER_RAD (180.0/M_PI)


using namespace BIAS;
using namespace std;


IndexedFaceSet::IndexedFaceSet() :
  writeOutTexture(false), bbmin(0,0,0), bbmax(0,0,0)
{
  NormalArray = NULL;
  useNormals = false;
  useTexture = true;
  useColoredVertices = false;
  CASize = VASize = 0;
#ifdef BIAS_HAVE_OPENGL
  compiled_ = false;
  NumFacets = 0;
  TASize = 0;
  displayList_ = 0;
  textureObjectID_ = 0;
  VertArray = NULL;
  TextureCoordArray = NULL;
  facetArray = NULL;
  ColorArray = NULL;
  facetIndicesCountArray = NULL;
#endif
}

IndexedFaceSet::IndexedFaceSet(const IndexedFaceSet& a)
{
  VASize = a.VASize;
  CASize = a.CASize;
  NormalArray = NULL;

#ifdef BIAS_HAVE_OPENGL
  TASize = a.TASize;
  compiled_ = false;
  displayList_ = 0;
  textureObjectID_ = 0,
  VertArray = NULL;
  TextureCoordArray = NULL;
  facetArray = NULL;
  facetIndicesCountArray = NULL;
  ColorArray = NULL;

  if (a.VertArray!=NULL) {
    VertArray = new GLfloat [VASize];
    memcpy(VertArray, a.VertArray, VASize*sizeof(GLfloat));
  }

  if (a.NormalArray!=NULL) {
    NormalArray = new GLfloat [VASize];
    memcpy(NormalArray, a.NormalArray, VASize*sizeof(GLfloat));
  }

  if (a.ColorArray!=NULL) {
    ColorArray = new GLubyte [CASize];
    memcpy(ColorArray, a.ColorArray, CASize*sizeof(GLubyte));
  }

  if (a.TextureCoordArray!=NULL) {
    TextureCoordArray = new GLfloat [TASize];
    memcpy(TextureCoordArray, a.TextureCoordArray, TASize*sizeof(GLfloat));
  }

  NumFacets = a.NumFacets;
  if(a.facetIndicesCountArray!=NULL) {
    facetIndicesCountArray = new GLsizei[NumFacets];
    memcpy(facetIndicesCountArray, a.facetIndicesCountArray,
           NumFacets*sizeof(GLsizei));
  }

  if(a.facetArray!=NULL) {
    facetArray = new GLuint*[NumFacets];
    for(int i=0; i<NumFacets; i++) {
      facetArray[i] = new GLuint[facetIndicesCountArray[i]];
      for(GLsizei j=0; j<facetIndicesCountArray[i]; j++)
        facetArray[i][j] = a.facetArray[i][j];
//      memcpy(facetArray[i], a.facetArray[i],
//      facetIndicesCountArray[i]*sizeof(GLuint));
    }
  }
#else
  if (a.NormalArray!=NULL) {
    NormalArray = new float [VASize];
    memcpy(NormalArray, a.NormalArray, VASize*sizeof(float));
  }
#endif
  texture = a.texture;
  vertexSet = a.vertexSet;
  texCoordSet = a.texCoordSet;
  faceIndices = a.faceIndices;
  vertexColorSet = a.vertexColorSet;
  name = a.name;
  textureName = a.textureName;
  writeOutTexture = a.writeOutTexture;
  useTexture = a.useTexture;
  bbmin  = a.bbmin;
  bbmax  = a.bbmax;
}

IndexedFaceSet::~IndexedFaceSet()
{
  if(NormalArray!=NULL) delete [] NormalArray;
#ifdef BIAS_HAVE_OPENGL
  if(VertArray!=NULL) delete [] VertArray;
  if(ColorArray!=NULL) delete [] ColorArray;
  if(TextureCoordArray!=NULL) delete [] TextureCoordArray;
  for(int i=0; i<NumFacets; i++){
    if(facetArray[i]!=NULL) delete[] facetArray[i];
  }
  if(facetArray!=NULL) delete [] facetArray;
  if(facetIndicesCountArray!=NULL) delete [] facetIndicesCountArray;
  // TODO fixme: glDeleteLists segfaults, test for displayList_ == null added as hotfix
  if (compiled_ && displayList_ != 0) {
    glDeleteLists(displayList_, 1);
    glDeleteTextures(1, &textureObjectID_);
  }
#endif
}

ThreeDOutParameters::
ThreeDOutParameters()
{
  PointStyle                = Point;
  CameraStyle               = PyramidMesh;
  LineStyle                 = Solid;
  DrawUncertaintyEllipsoids = false;
  DrawCameraUncertaintyEllipsoids = false;
  PointSize                 = 0.1;
  CameraScale               = 1.0;
  LineWidth                 = 1;
  EllipsoidScale            = 1.0;
  WCSAxesLength             = -1.0;
#ifdef BIAS_HAVE_OPENGL
  IndexedFaceSetPrimitiveMode = GL_POLYGON;
#endif
}

ThreeDOut::
ThreeDOut(const ThreeDOutParameters& params){
  NextPointIndex_     = 0;
  NextTextIndex_      = 0;
  NextLineIndex_      = 0;
  NextEllipsoidIndex_ = 0;
  NextCamEllipsoidIndex_ = 0;
  NextPIndex_         = 0;
  NextImageIndex_     = 0;
  NextIndexedFaceSetIndex_ = 0;
  WriteViewpoints_    = false;
  visualize_viewports = true;

  SetParams(params);

}

ThreeDOut::
~ThreeDOut(){}

void ThreeDOut::
SetParams(const ThreeDOutParameters& params){
  Params_ = params;
}

void ThreeDOut::
SetParamsPointSize(const double pointSize)
{
  Params_.PointSize = pointSize;
}
void ThreeDOut::
SetParamsLineWidth(const int lineWidth)
{
  Params_.LineWidth = lineWidth;
}

void ThreeDOut::
SetParamsCameraScale(const double cameraScale)
{
  Params_.CameraScale = cameraScale;
}

void ThreeDOut::
SetParamsCameraStyle(const CameraDrawingStyle cameraStyle)
{
  Params_.CameraStyle = cameraStyle;
}

void ThreeDOut::
SetParamsDrawEllipse(const bool drawEllipse)
{
  Params_.DrawUncertaintyEllipsoids = drawEllipse;
}

void ThreeDOut::
SetParamsDrawCameraEllipse(const bool drawEllipse)
{
  Params_.DrawCameraUncertaintyEllipsoids = drawEllipse;
}

void ThreeDOut::
SetParamsEllipsoidScale(const double ellipsoidScale)
{
  Params_.EllipsoidScale = ellipsoidScale;
}

void ThreeDOut::
SetParamsWCSAxesLength(double WCSAxesLength)
{
  Params_.WCSAxesLength = WCSAxesLength;
}

void ThreeDOut::
SetParamsPointDrawingStyle(const PointDrawingStyle pointStyle)
{
  Params_.PointStyle = pointStyle;
}

void ThreeDOut::
SetParamsLineDrawingStyle(const LineDrawingStyle lineStyle)
{
  Params_.LineStyle = lineStyle;
}

string ThreeDOut::
CorrectDefString_(const string& defstring) {
  if(defstring.size()==0)
    return defstring;
  string result = defstring;
  string::size_type posofinvalidchar = 0;
  if( isdigit(result[0]) !=0) {
    //BIASERR("DEF string in VRML must not start with a digit !");
    result = "_"+result;
  }
  if (result.size()>0) do {
    // just look for points (not allowed)
    posofinvalidchar = result.find('.', posofinvalidchar);
    if (posofinvalidchar != string::npos) {
      //BIASERR("replaced invalid character with underscore");
      result.at(posofinvalidchar) = '_';
    }
  } while (posofinvalidchar != string::npos);
  posofinvalidchar = 0;
  if (result.size()>0) do {
    // just look for points (not allowed)
    posofinvalidchar = result.find(' ', posofinvalidchar);
    if (posofinvalidchar != string::npos) {
      //BIASERR("replaced invalid character with underscore");
      result.at(posofinvalidchar) = '_';
    }
  } while (posofinvalidchar != string::npos);
  return result;
}

//This one writes described pyramid into VRMLFile
//r,g,b           - color of pyramid
//selectionRadius - length of Pyramid-EDGE
void ThreeDOut::
PaintPyramid(ostream& VRMLFile,
             double selectionRadius,
             double r, double g, double b, double transparency,
             double angle,
             double ratio,
             double scale,
             const std::string& name,
             bool paint_lines)
{
  double PI_1 = 3.1415926;
  double PI_2 = 6.2831852;

  bool angle_too_big = false;
  if(angle > PI_1 && angle < PI_2){
    angle_too_big = true;
    angle = PI_2-angle;
  }
  else if(angle > PI_2){
    return;
  }

  if (name.length()>0){
    std::string mod_name = CorrectDefString_(name);
    VRMLFile<<"DEF "<<mod_name << " ";
  }

  double dist_x = sin(angle/2.0);
  double dist_y = dist_x/(double)ratio;

  double dist = sqrt(dist_y*dist_y + 1.0);
  double k = (double)selectionRadius/dist;

  double dist_z = cos(angle/2.0)*k;
  dist_x = dist_x * k;
  dist_y = dist_y * k;

  //VRML ROUTINE
  VRMLFile<<"Transform {    "<<std::endl;
  VRMLFile<<"  translation 0 0 0"<<std::endl;
  //if (angle<=0.0) angle = 1.0;
  VRMLFile<<"  scale "<<scale<<" "<<scale
          <<" "<<scale<<" "<<endl;
  VRMLFile<<"  children ["<<std::endl;

  VRMLFile<<"    DEF ViewPort Transform {   "<<std::endl;
  if(angle_too_big){
    VRMLFile<<"      rotation 1 0 0 -1.57"<<std::endl;
    VRMLFile<<"      translation 0 0 0"<<std::endl;
  }
  else{
    VRMLFile<<"      rotation 1 0 0  1.57"<<std::endl;
    VRMLFile<<"      translation 0 0 0"<<std::endl;
  }
  VRMLFile<<"      children ["<<std::endl;

  //----------------FACES-----------------

  VRMLFile<<"        Shape { "<<std::endl;
  VRMLFile<<"          geometry IndexedFaceSet {"<<std::endl;

  VRMLFile<<"      "<<std::endl;

  VRMLFile<<"            coord Coordinate {"<<std::endl;
  VRMLFile<<"              point ["<<std::endl;

  VRMLFile<<"                0 0 0,"<<std::endl;

  VRMLFile<<"                "
          <<-dist_x<<" "<< dist_z<<" "<< dist_y<<","<<std::endl;
  VRMLFile<<"                "
          << dist_x<<" "<< dist_z<<" "<< dist_y<<","<<std::endl;
  VRMLFile<<"                "
          << dist_x<<" "<< dist_z<<" "<<-dist_y<<","<<std::endl;
  VRMLFile<<"                "
          <<-dist_x<<" "<< dist_z<<" "<<-dist_y<<","<<std::endl;
  VRMLFile<<"              ]"<<std::endl;
  VRMLFile<<"            }"<<std::endl;

  VRMLFile<<"            coordIndex ["<<std::endl;

  VRMLFile<<"              1, 2, 3, 4, -1,"<<std::endl;
  VRMLFile<<"              0, 1, 2, -1,"<<std::endl;
  VRMLFile<<"              0, 2, 3, -1,"<<std::endl;
  VRMLFile<<"              0, 3, 4, -1,"<<std::endl;
  VRMLFile<<"              0, 4, 1, -1,"<<std::endl;

  VRMLFile<<"            ]"<<std::endl;

  VRMLFile<<"          }"<<std::endl;
  VRMLFile<<"          appearance Appearance {"<<std::endl;
  VRMLFile<<"            material Material {"<<std::endl;
  VRMLFile<<"              emissiveColor "<<1.0<<" "<<1.0<<" "<<1.0<<std::endl;
  VRMLFile<<"              transparency "<<transparency<<std::endl;
  VRMLFile<<"            }"<<std::endl;
  VRMLFile<<"          }"<<std::endl;
  VRMLFile<<"        }"<<std::endl;

  //----------------LINES-----------------

  if(paint_lines){
    VRMLFile<<"        Shape { "<<std::endl;
    VRMLFile<<"          geometry IndexedLineSet {"<<std::endl;

    VRMLFile<<"      "<<std::endl;

    VRMLFile<<"            coord Coordinate {"<<std::endl;
    VRMLFile<<"              point ["<<std::endl;

    VRMLFile<<"                0 0 0,"<<std::endl;

    VRMLFile<<"                "
            <<-dist_x<<" "<< dist_z<<" "<< dist_y<<","<<std::endl;
    VRMLFile<<"                "
            << dist_x<<" "<< dist_z<<" "<< dist_y<<","<<std::endl;
    VRMLFile<<"                "
          << dist_x<<" "<< dist_z<<" "<<-dist_y<<","<<std::endl;
    VRMLFile<<"                "
            <<-dist_x<<" "<< dist_z<<" "<<-dist_y<<","<<std::endl;
    VRMLFile<<"              ]"<<std::endl;
    VRMLFile<<"            }"<<std::endl;

    VRMLFile<<"            color Color {"<<std::endl;
    VRMLFile<<"              color ["<<std::endl;
    VRMLFile<<"                255 255 255"<<std::endl;
    //<<r<<" "<<g<<" "<<b<<std::endl;
    VRMLFile<<"              ]"<<std::endl;
    VRMLFile<<"            }"<<std::endl;

    VRMLFile<<"            coordIndex ["<<std::endl;
    VRMLFile<<"              0, 1, -1,"<<std::endl;
    VRMLFile<<"              0, 2, -1,"<<std::endl;
    VRMLFile<<"              0, 3, -1,"<<std::endl;
    VRMLFile<<"              0, 4, -1,"<<std::endl;
    VRMLFile<<"            ]"<<std::endl;

    VRMLFile<<"            colorIndex ["<<std::endl;
    VRMLFile<<"              0, 0, 0, 0"<<std::endl;
    VRMLFile<<"            ]"<<std::endl;

    VRMLFile<<"          }"<<std::endl;
    VRMLFile<<"        }"<<std::endl;
  }

  //--------------------------------------

  VRMLFile<<"      ]"<<std::endl;
  VRMLFile<<"    }"<<std::endl;
  VRMLFile<<"  ]"<<std::endl;
  VRMLFile<<"}"<<std::endl;
}

//This one writes described cone into VRMLFile
//r,g,b           - color of Cone
//selectionRadius - length of Cone-SIDE
void ThreeDOut::
PaintCone(ostream& VRMLFile,
          double selectionRadius,
          double r, double g, double b, double transparency,
          double angle,
          double scale,
          const std::string& name)
{
  double PI_1 = 3.1415926;
  double PI_2 = 6.2831852;

  bool angle_too_big = false;
  if(angle > PI_1 && angle < PI_2){
    angle_too_big = true;
    angle = PI_2-angle;
  }
  else if(angle > PI_2){
    BIASERR("angle too big");
    return;
  }

  if (name.length()>0){
    string mod_name = CorrectDefString_(name);
    VRMLFile<<"DEF "<<mod_name << " ";
  }

  VRMLFile<<"Transform {    "<<std::endl;
  VRMLFile<<"  translation 0 0 0"<<std::endl;
  VRMLFile<<"  scale "<<scale<<" "<<scale
          <<" "<<scale<<" "<<endl;
  VRMLFile<<"  children ["<<std::endl;

  double radius = selectionRadius*sin(angle/2.0);
  double height = 0.0;
  if(angle != 1.0 && angle != 0.0)
    height = selectionRadius*cos(angle/2.0);

  VRMLFile<<"    DEF ViewPort Transform {   "<<std::endl;
  if(angle_too_big){
    VRMLFile<<"      rotation 1 0 0 1.57"<<std::endl;
    VRMLFile<<"      translation 0 0 "<<-height/2.0<<std::endl;
  }
  else{
    VRMLFile<<"      rotation 1 0 0 -1.57"<<std::endl;
    VRMLFile<<"      translation 0 0 "<<height/2.0<<std::endl;
  }
  VRMLFile<<"      children ["<<std::endl;
  VRMLFile<<"        Shape { "<<std::endl;
  VRMLFile<<"          geometry Cone{ bottomRadius "<<radius
          <<"            height "<<height<<std::endl
          <<"            side TRUE"<<std::endl
          <<"            bottom FALSE"<<std::endl
          <<"          }"<<std::endl;
  VRMLFile<<"          appearance Appearance {"<<std::endl;
  VRMLFile<<"            material Material {"<<std::endl;
  VRMLFile<<"              emissiveColor "<<1.0<<" "<<1.0<<" "<<1.0<<std::endl;
  VRMLFile<<"              transparency "<<transparency<<std::endl;
  VRMLFile<<"            }"<<std::endl;
  VRMLFile<<"          }"<<std::endl;
  VRMLFile<<"        }"<<std::endl;
  VRMLFile<<"      ]"<<std::endl;
  VRMLFile<<"    }"<<std::endl;


  VRMLFile<<"  ]"<<std::endl;
  VRMLFile<<"}"<<std::endl;
}

//Paints Conical representation of a perspective-viewport
void ThreeDOut::
VRMLOutViewPortPyramidal(ostream& VRMLFile,
                         double selectionRadius,
                         double r, double g, double b,
                         double angle,
                         double ratio,
                         double scale,
                         const std::string& name,
                         bool solid){
  //MAIN VIEWPORT
  if(solid){
    PaintPyramid(VRMLFile, selectionRadius, r, g, b, 0.0, angle,
                 ratio, scale, name, true);
  }
  else{
    PaintPyramid(VRMLFile, selectionRadius, r, g, b, 0.6, angle,
                 ratio, scale, name, true);
  }

  //each sub-pyramid represents 45, 90, 135, ... grade angle-view
  int iterations = (int)(angle/(3.1415926/4.0));

  for (int j = 0; j < iterations; j++){
    PaintPyramid(VRMLFile,
                 selectionRadius,
                 r, g, b, 0.95,
                 (3.1415926/4.0)*(double)(j+1),
                 ratio,
                 scale,
                 name);
  }
}

//Paints Conical representation of a non-perspective-viewport
void ThreeDOut::
VRMLOutViewPortConical(ostream& VRMLFile,
                       double selectionRadius,
                       double r, double g, double b,
                       double angle,
                       double scale,
                       const std::string& name,
                       bool solid){
        cout<<" trying to build conical camera rep:\n! ";
  //MAIN VIEWPORT
  if(solid){
    PaintCone(VRMLFile, selectionRadius, r, g, b, 0.0, angle,
            scale, name);
  }
  else{
    PaintCone(VRMLFile, selectionRadius, r, g, b, 0.6, angle,
            scale, name);
  }

  //each sub-cone represents 45, 90, 135, ... grade angle-view
  cout<<"angle "<<angle<<endl;
  int iterations = (int)(angle/(3.1415926/4.0));
  cout<<"iterations = "<<iterations<<endl;

  for (int j = 0; j < iterations; j++){
    PaintCone(VRMLFile,
              selectionRadius,
              r, g, b, 0.95,
              (3.1415926/4.0)*(double)(j+1),
              scale,
              name);
  }

}

void ThreeDOut::
CoordinateSystem(ostream& VRMLFile,
                 double selectionRadius,
                 double r, double g, double b, double axeslength,
                 double scale,
                 const std::string& name)
{

  //axeslength *= selectionRadius;

  if (name.length()>0){
    std::string mod_name = CorrectDefString_(name);
    VRMLFile<<"DEF "<<mod_name << " ";
  }

  VRMLFile<<"Transform {    "<<std::endl;
  if (axeslength<=0.0) axeslength = 1.0;
  VRMLFile<<"  scale "<<scale<<" "<<scale
          <<" "<<scale<<" "<<endl;
  VRMLFile<<"  children [ "<<std::endl;

  VRMLFile<<"    Shape {  "<<std::endl;
  /**
  VRMLFile<<"         geometry Box{ size "<<selectionRadius<<" "
    <<selectionRadius<<" "<<selectionRadius<<"  }  "<<std::endl;
  **/
  VRMLFile<<"      geometry Box{ size 0.1 0.1 0.1 }  "<<std::endl;
  VRMLFile<<"      appearance Appearance {"<<std::endl;
  VRMLFile<<"        material Material {"<<std::endl;
  VRMLFile<<"          emissiveColor "<<r<<" "<<g<<" "<<b<<std::endl;
  VRMLFile<<"        }"<<std::endl;
  VRMLFile<<"      }"<<std::endl;
  VRMLFile<<"    }"<<std::endl;

  VRMLFile<<"    DEF ArrowBody Transform {  "<<std::endl;
  VRMLFile<<"      translation 0.5 0 0"<<std::endl;
  VRMLFile<<"      children ["<<std::endl;
  VRMLFile<<"        Shape { "<<std::endl;
  VRMLFile<<"          geometry Box{ size "<<1.0<<" "<<0.05<<" "<<0.05<<
    " }  "<<std::endl;
  VRMLFile<<"          appearance Appearance {"<<std::endl;
  VRMLFile<<"            material Material {"<<std::endl;
  VRMLFile<<"              emissiveColor "<<r<<" "<<g<<" "<<b<<std::endl;
  VRMLFile<<"            }"<<std::endl;
  VRMLFile<<"          }"<<std::endl;
  VRMLFile<<"        }"<<std::endl;
  VRMLFile<<"      ]"<<std::endl;
  VRMLFile<<"    }"<<std::endl;

  VRMLFile<<"    Transform {"<<std::endl;
  VRMLFile<<"      rotation 0 1 0 -1.57"<<std::endl;
  VRMLFile<<"      children USE ArrowBody"<<std::endl;
  VRMLFile<<"    }"<<std::endl;

  VRMLFile<<"    Transform {"<<std::endl;
  VRMLFile<<"      rotation 0 0 1 1.57"<<std::endl;
  VRMLFile<<"      children USE ArrowBody"<<std::endl;
  VRMLFile<<"    }"<<std::endl;

  double bRad = 0.05;
  double hght = 0.20;

  VRMLFile<<"    Transform {"<<std::endl;
  VRMLFile<<"      translation 1 0 0"<<std::endl;
  VRMLFile<<"      rotation 0 0 1 -1.57"<<std::endl;
  VRMLFile<<"      children ["<<std::endl;
  VRMLFile<<"        Shape {"<<std::endl;
  VRMLFile<<"          geometry Cone{ "<<std::endl;
  VRMLFile<<"            bottomRadius "<<bRad<<" "<<std::endl;
  VRMLFile<<"            height "<<hght<<std::endl;
  VRMLFile<<"          }"<<std::endl;
  VRMLFile<<"          appearance Appearance {"<<std::endl;
  VRMLFile<<"            material Material {"<<std::endl;
  VRMLFile<<"              emissiveColor 1.0 0.0 0.0"<<std::endl;
  VRMLFile<<"            }"<<std::endl;
  VRMLFile<<"          }"<<std::endl;
  VRMLFile<<"        }"<<std::endl;
  VRMLFile<<"      ]"<<std::endl;
  VRMLFile<<"    }"<<std::endl;

  VRMLFile<<"    Transform {"<<std::endl;
  VRMLFile<<"      translation 0 1 0"<<std::endl;
  VRMLFile<<"      children ["<<std::endl;
  VRMLFile<<"        Shape {"<<std::endl;
  VRMLFile<<"          geometry Cone{ "<<std::endl;
  VRMLFile<<"            bottomRadius "<<bRad<<" "<<std::endl;
  VRMLFile<<"            height "<<hght<<std::endl;
  VRMLFile<<"          }"<<std::endl;
  VRMLFile<<"          appearance Appearance {"<<std::endl;
  VRMLFile<<"            material Material {"<<std::endl;
  VRMLFile<<"              emissiveColor 0.0 1.0 0.0"<<std::endl;
  VRMLFile<<"            }"<<std::endl;
  VRMLFile<<"          }"<<std::endl;
  VRMLFile<<"        }"<<std::endl;
  VRMLFile<<"      ]"<<std::endl;
  VRMLFile<<"    }"<<std::endl;

  VRMLFile<<"    Transform {"<<std::endl;
  VRMLFile<<"      translation 0 0 1"<<std::endl;
  VRMLFile<<"      rotation 1 0 0 1.57"<<std::endl;
  VRMLFile<<"      children ["<<std::endl;
  VRMLFile<<"        Shape {"<<std::endl;
  VRMLFile<<"          geometry Cone{ "<<std::endl;
  VRMLFile<<"            bottomRadius "<<bRad<<" "<<std::endl;
  VRMLFile<<"            height "<<hght<<std::endl;
  VRMLFile<<"          }"<<std::endl;
  VRMLFile<<"          appearance Appearance {"<<std::endl;
  VRMLFile<<"            material Material {"<<std::endl;
  VRMLFile<<"              emissiveColor 0.0 0.0 1.0"<<std::endl;
  VRMLFile<<"            }"<<std::endl;
  VRMLFile<<"          }"<<std::endl;
  VRMLFile<<"        }"<<std::endl;
  VRMLFile<<"      ]"<<std::endl;
  VRMLFile<<"    }"<<std::endl;

  VRMLFile<<"  ]"<<std::endl;
  VRMLFile<<"}"<<std::endl;
}

// ------------------------ 3d add data functions ------------------------

unsigned int ThreeDOut::AddPoint(const BIAS::Vector3<double> &v,
                                 const BIAS::RGBAuc &Color)
{
  Point3DData D;
  D.Color = Color;
  D.Point = v;
  mapPoints_[NextPointIndex_] = D;
  return NextPointIndex_++;
}

unsigned int ThreeDOut::AddPoint(const BIAS::HomgPoint3D &v,
                                 const BIAS::RGBAuc &Color)
{
#ifdef BIAS_DEBUG
  if (!v.IsHomogenized()) BIASERR("Supplied unhomogenized point !");
#endif
  Vector3<double> Center(v[0], v[1], v[2]);
  return AddPoint(Center, Color);
}

unsigned int ThreeDOut::AddText(const std::string& thetext,
                                const BIAS::Vector3<double> &position,
                                const BIAS::Quaternion<double> &direction,
                                const double& thesize,
                                const BIAS::RGBAuc &Color)
{
  Text3DData D;
  D.Color = Color;
  D.Position = position;
  D.Direction = direction;
  D.Size = thesize;
  D.Text=thetext;
  mapTexts_[NextTextIndex_] = D;
  return NextTextIndex_++;
}


unsigned int ThreeDOut::AddLine(const BIAS::Vector3<double> &Start,
                                const BIAS::Vector3<double> &End,
                                const BIAS::RGBAuc &Color)
{
  Line3DData D;
  D.Start = Start;
  D.End   = End;
  D.Color = Color;

  mapLines_[NextLineIndex_] = D;
  return NextLineIndex_++;
}

unsigned int ThreeDOut::AddLine(const BIAS::HomgPoint3D &Start,
                                const BIAS::HomgPoint3D &End,
                                const BIAS::RGBAuc &Color)
{
#ifdef BIAS_DEBUG
  if (!Start.IsHomogenized()) BIASERR("Supplied unhomogenized start point !");
  if (!End.IsHomogenized()) BIASERR("Supplied unhomogenized end point !");
#endif
  Vector3<double> s(Start[0], Start[1], Start[2]);
  Vector3<double> e(End[0], End[1], End[2]);
  return AddLine(s, e, Color);
}

unsigned int ThreeDOut::AddEllipsoid(BIAS::CovMatrix3x3 &cov,
                                     const BIAS::Vector3<double> &C,
                                     const BIAS::RGBAuc &Color,
                                     const double& dConfidenceRegionScale,
                                     const bool CamEllipsoid)
{
  Ellipsoid3DData Data;
  Matrix3x3<double> VT(MatrixZero);
  RMatrix VInv(MatrixIdentity);
  Vector3<double> S;
  int resPCA = cov.GetPCA(S, VT);
  if (resPCA == 0 && BIAS::Greater(VT.GetRow(0).NormL2(), 0.0, 1e-8) &&
      BIAS::Greater(VT.GetRow(1).NormL2(), 0.0, 1e-8) &&
      BIAS::Greater(VT.GetRow(2).NormL2(), 0.0, 1e-8)) {
    // assure that PCA results in a valid rotation matrix
    VInv = VT.Transpose();
  } else if (!VT.IsZero(1e-8)) {
    // show warning only if we don't have the zero matrix
    BIASWARN("Degenerated rotation matrix VT computed from covariance matrix "
             "via PCA! Using default covariance ellipsoid alignment!" << endl
             << "- degenerated matrix VT = " << VT);
  }
  VInv.GetRotationAxisAngle(Data.RotAxis, Data.dAngle);
  Data.s0 = sqrt(S[0]) * dConfidenceRegionScale;
  Data.s1 = sqrt(S[1]) * dConfidenceRegionScale;
  Data.s2 = sqrt(S[2]) * dConfidenceRegionScale;
  Data.C = C;
  Data.Color = Color;
  // @todo Assure that y and z can't be exchanged by SVD accidently!

  int index = 0;
  if (CamEllipsoid){
    mapCamEllipsoids_[NextCamEllipsoidIndex_] = Data;
    index = NextCamEllipsoidIndex_++;
  } else {
    mapEllipsoids_[NextEllipsoidIndex_] = Data;
    index = NextEllipsoidIndex_++;
  }
  return index;
}

unsigned int ThreeDOut::AddEllipsoid(BIAS::CovMatrix3x3 &cov,
                                     const BIAS::HomgPoint3D &C,
                                     const BIAS::RGBAuc &Color,
                                     const double &dConfidenceRegionScale,
                                     const bool CamEllipsoid)
{
#ifdef BIAS_DEBUG
  if (!C.IsHomogenized())
    BIASERR("Supplied unhomogenized point!");
#endif
  Vector3<double> Center(C[0], C[1], C[2]);
  return AddEllipsoid(cov, Center, Color, dConfidenceRegionScale, CamEllipsoid);
}

void ThreeDOut::AddPlane(const HomgPlane3D& plane,
                         const RGBAuc& colour,
                         const double size,
                         const Vector3<double>& translation,
                         const bool wireframe)
{
  // compute corners of plane
  std::vector< Vector3<double> > corners;
  ThreeDOut::ComputePlaneCorners(plane, size, corners);

  // translate plane points by given vector
  for (int i = 0; i < 4; i++)
    corners[i] += translation;

  // finally create visualisation
  if (wireframe)
  {
    for (int i = 0; i < 3; i++)
      for (int j = i + 1; j < 4; j++)
        AddLine(corners[i], corners[j], colour);
  }
  else
  {
    const Vector3<double>& normal = plane.GetNormalVector();
    std::vector< Vector3<double> > vertexNormals(4);
    std::vector< Vector4<unsigned char> > vertexColors(4);
    for (int i = 0; i < 4; i++) {
      vertexNormals[i] = normal;
      vertexColors[i] = colour;
    }
    std::vector< Vector<int> > triangleIndices(2);
    triangleIndices[0] = Vector<int>(Vector3<int>(0, 1, 2));
    triangleIndices[1] = Vector<int>(Vector3<int>(2, 1, 3));
    BIAS::TriangleMesh quad;
    quad.SetMesh(corners, vertexNormals, vertexColors, triangleIndices);
    AddTriangleMesh(quad);
  }
}

void ThreeDOut::
ComputePlaneCorners(const HomgPlane3D& plane, const double size,
                    std::vector< Vector3<double> >& corners)
{
  // some constants for better readability
  const Vector3<double>& normal = plane.GetNormalVector();
  const double halfSize = size / 2.0;

  // find coordinate axis that is closest to 90 degrees to the plane normal
  // and create four points in the plane definded by the coordinate axis as
  // normal. this step is necessary for the cross product below
  const double optimalAngle = M_PI / 2.0; // = 90 degrees

  const Vector3<double> xAxis(1.0, 0.0, 0.0);
  const Vector3<double> yAxis(0.0, 1.0, 0.0);
  const Vector3<double> zAxis(0.0, 0.0, 1.0);

  const double angleX = acos(xAxis.ScalarProduct(normal));
  const double angleY = acos(yAxis.ScalarProduct(normal));
  const double angleZ = acos(zAxis.ScalarProduct(normal));

  const double diffAngleX = fabs(optimalAngle - angleX);
  const double diffAngleY = fabs(optimalAngle - angleY);
  const double diffAngleZ = fabs(optimalAngle - angleZ);

  Vector3<double> axis;
  double angle;
  Vector3<double> p1;
  Vector3<double> p2;
  Vector3<double> p3;
  Vector3<double> p4;

  if (diffAngleX <= diffAngleY && diffAngleX <= diffAngleZ) { // choose x axis
    angle = angleX;
    axis  = xAxis;
    p1[0] = 0.0; p1[1] = -halfSize; p1[2] =  halfSize;
    p2[0] = 0.0; p2[1] =  halfSize; p2[2] =  halfSize;
    p3[0] = 0.0; p3[1] = -halfSize; p3[2] = -halfSize;
    p4[0] = 0.0; p4[1] =  halfSize; p4[2] = -halfSize;
  }
  else if (diffAngleY <= diffAngleZ) {                        // choose y axis
    angle = angleY;
    axis  = yAxis;
    p1[0] = -halfSize; p1[1] = 0.0; p1[2] =  halfSize;
    p2[0] =  halfSize; p2[1] = 0.0; p2[2] =  halfSize;
    p3[0] = -halfSize; p3[1] = 0.0; p3[2] = -halfSize;
    p4[0] =  halfSize; p4[1] = 0.0; p4[2] = -halfSize;
  }
  else {                                                      // choose z axis
    angle = angleZ;
    axis  = zAxis;
    p1[0] = -halfSize; p1[1] =  halfSize; p1[2] = 0.0;
    p2[0] =  halfSize; p2[1] =  halfSize; p2[2] = 0.0;
    p3[0] = -halfSize; p3[1] = -halfSize; p3[2] = 0.0;
    p4[0] =  halfSize; p4[1] = -halfSize; p4[2] = 0.0;
  }

  // compute rotation so that normal points into direction of chosen axis and
  // apply the inverse rotation on points
  const Vector3<double> rotationAxis = axis.CrossProduct(normal);

  const RMatrix rotation(rotationAxis, -angle);
  RMatrix invRotation;
  rotation.GetInverse(invRotation);

  p1 = invRotation * p1;
  p2 = invRotation * p2;
  p3 = invRotation * p3;
  p4 = invRotation * p4;

  // move points by the distance between plane and origin, they now lie on plane
  const double distance = -plane[3];
  const Vector3<double> scaledNormal = distance * normal;

  p1 += scaledNormal;
  p2 += scaledNormal;
  p3 += scaledNormal;
  p4 += scaledNormal;

  // return vertices
  corners.resize(4);
  corners[0] = p1;
  corners[1] = p2;
  corners[2] = p3;
  corners[3] = p4;
}

unsigned int ThreeDOut::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,
                                 const std::string& name,
                                 bool billboard)
{
#ifdef BIAS_DEBUG
  if (Width>UINT_MAX/2) {
    BIASERR("Width "<<Width<<" is out of bounds !");
    BIASABORT;
  }
  if (Height>UINT_MAX/2) {
    BIASERR("Height "<<Height<<" is out of bounds !");
    BIASABORT;
  }
  if ((channels!=1)&&(channels!=3)) {
    BIASERR("Unknown channel count:"<<channels);
    BIASABORT;
  }
#endif
  // find min w with w>=width and w=2^n, needed by opengl
  unsigned int w(1), h(1);
  while (w<Width)  w<<=1;
  while (h<Height) h<<=1;

  Image3DData &ImagePatch = mapImages_[NextImageIndex_];
  if (!ImagePatch.ImageData.IsEmpty()) ImagePatch.ImageData.Release();
  ImagePatch.ImageData.Init(w, h, channels);
  ImagePatch.ImageData.SetROI(0, 0, Width, Height);
  unsigned char** pColRow = ImagePatch.ImageData.GetImageDataArray();
  unsigned char* pOrig = pData;
  for (unsigned int i=0; i<Height; i++) {
    memcpy(pColRow[i], pOrig,  channels*Width*sizeof(unsigned char));
    pOrig += channels*Width;
  }
  ImagePatch.UL = UL;
  ImagePatch.UR = UR;
  ImagePatch.LL = LL;
  ImagePatch.LR = LR;
  ImagePatch.name = CorrectDefString_(name);
  ImagePatch.BillBoard = billboard;
  return NextImageIndex_++;
}


// wrapper
unsigned int ThreeDOut::AddPMatrix(BIAS::Matrix3x4<double> &M,
                                   const unsigned int& width,
                                   const unsigned int& height,
                                   const BIAS::RGBAuc &Color,
                                   const double &dScale,
                                   const string& name)
{
  PMatrix P( M );
  return AddPMatrix(P,
    width,
    height,
    Color,
    dScale,
    name);
}


unsigned int ThreeDOut::AddPMatrix(BIAS::PMatrix &P,
                                   const unsigned int& width,
                                   const unsigned int& height,
                                   const BIAS::RGBAuc &Color,
                                   const double &dScale,
                                   const string& name)
{
  Vector3<double> d,A;
  HomgPoint2D ThisCorner(0.0, 0.0, 0.0);
  Matrix3x3<double> R, iKRt;
  KMatrix K;
  PMatrix3DData pd;
  pd.name = CorrectDefString_(name);
  pd.Color=Color;
  P.GetK(K);
  P.GetR(R);
  P.GetC(pd.C);
  P.GetA(A);
  int ires = (K * R.Transpose()).GetInverse(iKRt);
  if (ires!=0) BIASERR("error computing iKRt.");
  BIASDOUT(D_3DOUT_PDECOMP, "Decomposing P="<<P<<" into K="<<K<<" R="
    <<R<<" C="<<pd.C<<" A="<<A);
  for (unsigned int i=0; i<5; i++){
    switch (i) {
    case 0:
      ThisCorner[0] = 0.0;
      ThisCorner[1] = 0.0;
      break;
    case 1:
      ThisCorner[0] = width-1;
      ThisCorner[1] = 0.0;
      break;
    case 2:
      ThisCorner[0] = width-1;
      ThisCorner[1] = height-1;
      break;
    case 3:
      ThisCorner[0] = 0.0;
      ThisCorner[1] = height-1;
      break;
    case 4:
      ThisCorner[0] = K[0][2];
      ThisCorner[1] = K[1][2];
      break;
    }
    ThisCorner[2] = 1.0;
    // get backprojection ray d from camera center to image point ThisCorner
    iKRt.Mult(ThisCorner,d);

    // normalize projection of d on A to length 1
    d *= dScale / (d.ScalarProduct(A));

    // attach ray d to camera center to construct a 3d point
    d += pd.C;
    switch (i) {
    case 0: pd.UL=d; break;
    case 1: pd.UR=d; break;
    case 2: pd.LR=d; break;
    case 3: pd.LL=d; break;
    case 4: pd.PP=d; break;
    }
  } // for (unsigned int i=0; i<4; i++){
  BIASDOUT(D_3DOUT_PDECOMP, "Created points ul="<<pd.UL<<" ur="<<pd.UR<<" lr="
    <<pd.LR<<" ll="<<pd.LL<<" and principle point at "<<pd.PP);
  pd.Up[0] = R[0][1];
  pd.Up[1] = R[1][1];
  pd.Up[2] = R[2][1];
  pd.Up *= -dScale /2.0;
  mapPMatrices_[NextPIndex_] = pd;


  return NextPIndex_++;
}

unsigned int ThreeDOut::
AddTriangleMesh(const BIAS::TriangleMesh& mesh,
                const std::string& name,
                const std::string& textureOutputName,
                bool writeOutTexture,
                bool calcNormals)
{
  IndexedFaceSet& Data = mapIndexedFaceSet_[NextIndexedFaceSetIndex_];
  Data.name = CorrectDefString_(name);
  Data.textureName = textureOutputName;
  Data.writeOutTexture = writeOutTexture;

  //use texture
  mesh.GetTexture(Data.texture);
  Data.useTexture = Data.texture.GetWidth() > 0;

  //use colors per vertex instead of texture
  mesh.GetVertexColors(Data.vertexColorSet);
  Data.useColoredVertices = Data.vertexColorSet.size() > 0;

  //get vertices, texture coordinates and face indices
  mesh.GetVertices(Data.vertexSet);
  mesh.GetTexCoords(Data.texCoordSet);
  mesh.GetTriangleIndices(Data.faceIndices);
  Data.useNormals = false;

  //assign sizes for arrays (3 per vertex, 4 per color, 2 per tex coordinate)
  Data.VASize = 3 * Data.vertexSet.size();
  Data.CASize = 4 * Data.vertexSet.size();

  if(Data.NormalArray != NULL) delete [] Data.NormalArray;

#ifdef BIAS_HAVE_OPENGL

  Data.TASize = Data.texCoordSet.size() * 2;

  if(Data.VertArray != NULL) delete[] Data.VertArray;
  if(Data.ColorArray != NULL) delete[] Data.ColorArray;
  if(Data.TextureCoordArray != NULL) delete[] Data.TextureCoordArray;

  Data.VertArray = new GLfloat[Data.VASize];
  Data.TextureCoordArray = new GLfloat[Data.TASize];
  Data.ColorArray = new GLubyte[Data.CASize];
  for (int i=0; i<Data.CASize; i++)
    Data.ColorArray[i] = 255; // use white vertices by default

  //assign color values
  if(Data.useColoredVertices)
  {
    //go over all vertices and assign colors for each vertex
    BIASASSERT(Data.vertexSet.size() == Data.vertexColorSet.size());
    for (unsigned int i=0; i<Data.vertexColorSet.size(); i++) {
      Vector4<unsigned char> color = Data.vertexColorSet[i];
      for (unsigned int j=0; j<4; j++)
        Data.ColorArray[i*4+j] = color[j];
    }
  }
#endif // BIAS_HAVE_OPENGL

  //take original normals from triangle mesh or recompute them or do nothing
  vector<Vector3<double> > normals;
  mesh.GetNormals(normals);  
  if (!normals.empty() && !calcNormals) // && !Data.useTexture)
  {
    //use normals from mesh
#ifdef BIAS_HAVE_OPENGL
    Data.NormalArray = new GLfloat[Data.VASize];
#else
    Data.NormalArray = new float[Data.VASize];
#endif
    //set all values to zero
    for (int i=0; i<Data.VASize; i++)
      Data.NormalArray[i] = 0;
    // copy normals
    BIASASSERT(normals.size() == Data.vertexSet.size());
    for (size_t normalsCnt = 0; normalsCnt < normals.size(); ++normalsCnt) {
      Data.NormalArray[3 * normalsCnt    ] = normals[normalsCnt][0];
      Data.NormalArray[3 * normalsCnt + 1] = normals[normalsCnt][1];
      Data.NormalArray[3 * normalsCnt + 2] = normals[normalsCnt][2];
    }
    Data.useNormals = true;
  }
  else if (calcNormals) { // && !Data.useTexture) {
    //recompute normals
#ifdef BIAS_HAVE_OPENGL
    Data.NormalArray = new GLfloat[Data.VASize];
#else
    Data.NormalArray = new float[Data.VASize];
#endif
    //set all values to zero
    for (int i=0; i<Data.VASize; i++)
      Data.NormalArray[i] = 0;
    //compute normals
    Vector3<double> v1,v2,v3;
    // run over all triangles and assign normal to vertex
    for (unsigned int i=0; i<Data.faceIndices.size(); i++) {
      //BIASASSERT(Data.faceIndices[i][0]<Data.vertexSet.size());
      //BIASASSERT(Data.faceIndices[i][1]<Data.vertexSet.size());
      //BIASASSERT(Data.faceIndices[i][2]<Data.vertexSet.size());
      v1 = Data.vertexSet[Data.faceIndices[i][0]];
      v2 = Data.vertexSet[Data.faceIndices[i][1]] - v1;
      v3 = Data.vertexSet[Data.faceIndices[i][2]] - v1;
      double norm = v2.NormL2();
      if (norm > 0.0) v2 /= norm;
      norm = v3.NormL2();
      if (norm > 0.0) v3 /= norm;
      // compute normal of two triangle edges
      v1 = v2.CrossProduct(v3);

      norm = v1.NormL2();
      if (norm > 0.0) v1 /= norm;
      for (unsigned int k=0; k<3; k++)
        Data.NormalArray[Data.faceIndices[i][0]*3+k] += v1[k];
      for (unsigned int k=0; k<3; k++)
        Data.NormalArray[Data.faceIndices[i][1]*3+k] += v1[k];
      for (unsigned int k=0; k<3; k++)
        Data.NormalArray[Data.faceIndices[i][2]*3+k] += v1[k];
    }

    for (unsigned int i=0; i<Data.vertexSet.size(); i++) {
      double norm = Data.NormalArray[3*i]* Data.NormalArray[3*i] +
                    Data.NormalArray[3*i+1]* Data.NormalArray[3*i+1] +
                    Data.NormalArray[3*i+2]* Data.NormalArray[3*i+2];
      if (!BIAS::Equal(norm, 1.0)) {
        if (norm>0.0) {
          norm = 1.0 / sqrt(norm);
          Data.NormalArray[3*i]   *= norm;
          Data.NormalArray[3*i+1] *= norm;
          Data.NormalArray[3*i+2] *= norm;
        } else {
          // no normal, the vertex is not found in ANY triangle, set (0,0,1) normal
          Data.NormalArray[3*i] = 0;
          Data.NormalArray[3*i+1] = 0;
          Data.NormalArray[3*i+2] = 1;
        }
      } else {
        BIASERR("Computed normal has norm " << norm << "!");
      }
    }
    Data.useNormals = true;
  }

#ifdef BIAS_HAVE_OPENGL
  //fill VertArray via pCurVertex
  GLfloat *pCurVertex = Data.VertArray;
#endif
  //calculate bounding box
  Data.bbmin[0] = Data.bbmin[1] = Data.bbmin[2] =  DBL_MAX;
  Data.bbmax[0] = Data.bbmax[1] = Data.bbmax[2] = -DBL_MAX;

  for (unsigned int v =0; v<Data.vertexSet.size(); v++) {
    const Vector3<double> &point = Data.vertexSet[v];
    if (Data.bbmin[0] > point[0]) Data.bbmin[0] = point[0];
    if (Data.bbmax[0] < point[0]) Data.bbmax[0] = point[0];

    if (Data.bbmin[1] > point[1]) Data.bbmin[1] = point[1];
    if (Data.bbmax[1] < point[1]) Data.bbmax[1] = point[1];

    if (Data.bbmin[2] > point[2]) Data.bbmin[2] = point[2];
    if (Data.bbmax[2] < point[2]) Data.bbmax[2] = point[2];

#ifdef BIAS_HAVE_OPENGL
    *(pCurVertex++) = (float)point[0];
    *(pCurVertex++) = (float)point[1];
    *(pCurVertex++) = (float)point[2];
#endif
  }

#ifdef BIAS_HAVE_OPENGL
  if (Data.useTexture) {
    unsigned int wold,hold,wnew,hnew;
    wold=Data.texture.GetWidth();
    hold=Data.texture.GetHeight();
    wnew=ImageBase::PowerOfTwoSize(wold);
    hnew=ImageBase::PowerOfTwoSize(hold);
    float sx=float(wold)/float(wnew);
    float sy=float(hold)/float(hnew);
    GLfloat *pCurTexture = Data.TextureCoordArray;
    for (unsigned int t =0; t<Data.texCoordSet.size(); t++) {
      const Vector2<float>& texCoord = Data.texCoordSet[t];
      *(pCurTexture++) = texCoord[0] * sx;
      *(pCurTexture++) = texCoord[1] * sy;
    }
  }

  //generate faces and index counts:
  Data.NumFacets = Data.faceIndices.size();
  if(Data.NumFacets>0) {
    Data.facetIndicesCountArray = new GLsizei[Data.NumFacets];
    Data.facetArray = new GLuint*[Data.NumFacets];
    for(int i=0; i<Data.NumFacets; i++) {
      Data.facetIndicesCountArray[i]=Data.faceIndices[i].dim();
      if(Data.facetIndicesCountArray[i]>0)
        Data.facetArray[i] = new GLuint[Data.facetIndicesCountArray[i]];
      for(GLsizei j=0; j<Data.facetIndicesCountArray[i]; j++) {
        Data.facetArray[i][j] = Data.faceIndices[i][j];
      }
    }
  }
#endif // BIAS_HAVE_OPENGL

  return NextIndexedFaceSetIndex_++;
}

unsigned int ThreeDOut::
AddProjection(const Projection&  p,
              const RGBAuc&      colorSelection,
              const double       radius,
              const double       scale,
              const std::string& identifier,
              const int          camera_type)
{

  Projection3DData data;
  unsigned int i=0;
  while(i<p.Size()){
    BIAS::Quaternion<double> Q,Q1;
    //This is the initial Pose
    //BIAS::ProjectionParametersBase *P1=NULL;
    BIAS::Vector3<double> C,C1;
    if(i==0){ // this is the camera the others refer to
      Q1 = Q = p.GetQ(i);
      C1 = C = p.GetC(i);
      //P1 = p.GetParameters()->Clone();
    }//successive cameras are relative to the first in the rig
    else{
      Pose absPose = p.GetParameters()->GetPose();
      Pose relPose;
      absPose.ConcatenateLocalTransform(p.GetParameters(i)->GetPose(),
                                        relPose);

      Q = relPose.GetQ();
      C = relPose.GetC();
      AddLine(C1, C, colorSelection);
    }

    RMatrix R;
    R.SetFromQuaternion(Q);
    data.C = C;
    data.Set(C, R, scale);

    //data.Set(*p.GetParameters(i), scale);
    data.radiusSelection = radius;
    data.ColorOfSelection = colorSelection;
    data.identifier = identifier;
    data.scale = scale;
    data.type = camera_type;

    //data.angle = 0.0;

    /** VIEWPORT PART, DUE TO BE IMPLEMENTED

    unsigned int wid = 0;
    unsigned int hei = 0;

    //calculate the angle
    if(camera_type != -1){
    //calculate the size of an image
    p.GetParameters()->GetImageSize(wid, hei);
    HomgPoint2D p1_2D, p2_2D;

    cout << "wid: " << wid << ", hei: " << hei << endl;

    if(hei==0 && wid==0){
    cout << "Unable to calculate size of an img from projection..." << endl;
    }
    else{
    if(camera_type == 0){
    p1_2D[0] =           0; p1_2D[1] = rint(hei/2);
    p2_2D[0] = rint(wid/2); p2_2D[1] = rint(hei/2);
    }
    if(camera_type == 1){
    //we take a point (wid/4, hei/2) and (wid/2, hei/2), 'cause
    //the angle could be more than 180 degrees
    p1_2D[0] = rint(wid/4); p1_2D[1] = rint(hei/2);
    p2_2D[0] = rint(wid/2); p2_2D[1] = rint(hei/2);
    }
    HomgPoint3D p1_3D = p.GetParameters()->UnProjectToPoint(p1_2D, 1.0);
    HomgPoint3D p2_3D = p.GetParameters()->UnProjectToPoint(p2_2D, 1.0);

    double dx = p1_3D[0]-p2_3D[0];
    double dy = p1_3D[1]-p2_3D[1];
    double dz = p1_3D[2]-p2_3D[2];

    //half of the distance between two 1.0-lentgh-rays from the principal point
    //it's equal to a sin of an alpha/2 for perspective and aplpha/4 for
    //non-perspective cameras, whereas alpha is an cutoffangle of the camera.
    double dist_div_2 = sqrt(dx*dx+dy*dy+dz*dz)/2;

    if(camera_type == 0)
    data.angle = 2*asin(dist_div_2);
    if(camera_type == 1)
    data.angle = 4*asin(dist_div_2);
    }
    }
    **/
    i++;
    mapProjection3D_[NextProjection3DIndex_++] = data;
  }

  return NextProjection3DIndex_;
}

unsigned int ThreeDOut::
AddPose(const PoseParametrization& p,
        const RGBAuc& colorSelection,
        const double radius,
        const double scale,
        const int    camera_type)
{
  Projection3DData data;
  data.Set(p, scale);
  data.radiusSelection = radius;
  data.ColorOfSelection = colorSelection;
  data.type = camera_type;

  mapProjection3D_[NextProjection3DIndex_] = data;
  return NextProjection3DIndex_++;
}


// -------------------------- 3D data update functions -----------------------


void ThreeDOut::UpdatePoint(const unsigned int& PointIndex,
                            const BIAS::Vector3<double> &v,
                            const BIAS::RGBAuc &Color)
{
#ifdef BIAS_DEBUG
  ASSERT_MAP_ELEMENT(mapPoints_, PointIndex);
#endif
  Point3DData& D = mapPoints_[PointIndex];
  D.Point = v;
  D.Color = Color;
}

void ThreeDOut::UpdatePointColor(const unsigned int& PointIndex,
                                 const BIAS::RGBAuc &Color)
{
  mapPoints_[PointIndex].Color = Color;
}

void ThreeDOut::UpdatePoint(const unsigned int& PointIndex,
                            const BIAS::HomgPoint3D &v,
                            const BIAS::RGBAuc &Color)
{
#ifdef BIAS_DEBUG
  if (!v.IsHomogenized()) BIASERR("Supplied unhomogenized point !");
#endif
  Vector3<double> V(v[0], v[1], v[2]);
  UpdatePoint(PointIndex, V, Color);
}

void ThreeDOut::UpdatePMatrix(const unsigned int& PIndex,
                              BIAS::PMatrix &P,
                              const unsigned int& width,
                              const unsigned int& height,
                              const BIAS::RGBAuc &Color,
                              const double& dScale)
{
  unsigned int backupnextpindex = NextPIndex_;
  NextPIndex_ = PIndex;
  AddPMatrix(P, width, height, Color, dScale);
  NextPIndex_ = backupnextpindex;
}

void ThreeDOut::UpdatePMatrixColor(const unsigned int& PIndex,
                                   const BIAS::RGBAuc &Color)
{
  mapPMatrices_[PIndex].Color = Color;
}

void ThreeDOut::UpdateLine(const unsigned int& LineIndex,
                           const BIAS::Vector3<double> &Start,
                           const BIAS::Vector3<double> &End,
                           const BIAS::RGBAuc &Color)
{
#ifdef BIAS_DEBUG
  ASSERT_MAP_ELEMENT(mapLines_, LineIndex);
#endif
  Line3DData& D = mapLines_[LineIndex];
  D.Start = Start;
  D.End = End;
  D.Color = Color;
}

void ThreeDOut::UpdateLine(const unsigned int& LineIndex,
                           const BIAS::HomgPoint3D &Start,
                           const BIAS::HomgPoint3D &End,
                           const BIAS::RGBAuc &Color)
{
#ifdef BIAS_DEBUG
  if (!Start.IsHomogenized()) BIASERR("Supplied unhomogenized start point !");
  if (!End.IsHomogenized()) BIASERR("Supplied unhomogenized end point !");
#endif
  Vector3<double> s(Start[0], Start[1], Start[2]);
  Vector3<double> e(End[0], End[1], End[2]);
  UpdateLine(LineIndex, s, e, Color);
}


void ThreeDOut::UpdateEllipsoid(const unsigned int& EIndex,
                                BIAS::CovMatrix3x3 &cov,
                                BIAS::Vector3<double> &C,
                                const BIAS::RGBAuc &Color,
                                const double &dConfidenceRegionScale)
{
#ifdef BIAS_DEBUG
  ASSERT_MAP_ELEMENT(mapEllipsoids_, EIndex);
#endif
  Ellipsoid3DData& Data = mapEllipsoids_[EIndex];
  Matrix3x3<double> VT(MatrixZero);
  RMatrix VInv(MatrixIdentity);
  Vector3<double> S;
  int resPCA = cov.GetPCA(S, VT);
  if (resPCA == 0 && BIAS::Greater(VT.GetRow(0).NormL2(), 0.0, 1e-8) &&
      BIAS::Greater(VT.GetRow(1).NormL2(), 0.0, 1e-8) &&
      BIAS::Greater(VT.GetRow(2).NormL2(), 0.0, 1e-8)) {
    // assure that PCA results in a valid rotation matrix
    VInv = VT.Transpose();
  } else if (!VT.IsZero(1e-8)) {
    // show warning only if we don't have the zero matrix
    BIASWARN("Degenerated rotation matrix VT computed from covariance matrix "
             "via PCA! Using default covariance ellipsoid alignment!" << endl
             << "- degenerated matrix VT = " << VT);
  }
  VInv.GetRotationAxisAngle(Data.RotAxis, Data.dAngle);
  Data.s0 = sqrt(S[0]) * dConfidenceRegionScale;
  Data.s1 = sqrt(S[1]) * dConfidenceRegionScale;
  Data.s2 = sqrt(S[2]) * dConfidenceRegionScale;
  Data.C = C;
  Data.Color = Color;
}

void ThreeDOut::UpdateEllipsoid(const unsigned int& EIndex,
                                BIAS::CovMatrix3x3 &cov,
                                BIAS::HomgPoint3D &C,
                                const BIAS::RGBAuc &Color,
                                const double& dConfidenceRegionScale)
{
#ifdef BIAS_DEBUG
  if (!C.IsHomogenized()) BIASERR("Supplied unhomogenized point !");
#endif
  Vector3<double> Center(C[0], C[1], C[2]);
  UpdateEllipsoid(EIndex, cov, Center, Color, dConfidenceRegionScale);
}

void ThreeDOut::RemovePoint(const unsigned int& PointIndex)
{
  ASSERT_MAP_ELEMENT(mapPoints_, PointIndex);
  // uncontrolled erase leads to crash in case of non existing PointIndex:
  mapPoints_.erase(mapPoints_.find(PointIndex));
}

void ThreeDOut::RemoveAllPoints()
{
  mapPoints_.clear();
}

void ThreeDOut::RemoveLine(const unsigned int& LineIndex)
{
  ASSERT_MAP_ELEMENT(mapLines_, LineIndex);
  // uncontrolled erase leads to crash in case of non existing PointIndex:
  mapLines_.erase(mapLines_.find(LineIndex));
}

void ThreeDOut::RemoveAllLines()
{
  mapLines_.clear();
}

void ThreeDOut::RemoveEllipsoid(const unsigned int& EIndex)
{
  ASSERT_MAP_ELEMENT(mapEllipsoids_, EIndex);
  // uncontrolled erase leads to crash in case of non existing PointIndex:
  mapEllipsoids_.erase(mapEllipsoids_.find(EIndex));
}

void ThreeDOut::RemoveAllEllipsoids() {
  mapEllipsoids_.clear();
}

void ThreeDOut::RemoveAllCamEllipsoids() {
  mapCamEllipsoids_.clear();
}

void ThreeDOut::RemoveImage(const unsigned int& EIndex)
{
  ASSERT_MAP_ELEMENT(mapImages_, EIndex);
  // uncontrolled erase leads to crash in case of non existing PointIndex:
  mapImages_.erase(mapImages_.find(EIndex));
}

void ThreeDOut::RemoveAllIndexedFaceSets()
{
  mapIndexedFaceSet_.clear();
  NextIndexedFaceSetIndex_ = 0;
}

void ThreeDOut::RemoveAllImages()
{
  mapImages_.clear();
}

void ThreeDOut::RemoveAllPMatrices()
{
  mapPMatrices_.clear();
}

void ThreeDOut::RemovePMatrix(int index)
{
  map<unsigned int, PMatrix3DData>::iterator it = mapPMatrices_.find(index);
  if (it!=mapPMatrices_.end()) {
    mapPMatrices_.erase(it);
  } else {
    BIASERR("Tried to erase non-existing pmatrix:"<<index);
  }
}

void ThreeDOut::VRMLOutWCS(ostream& VRMLFile) {
  if (Params_.WCSAxesLength>0.0)
    CoordinateSystem(VRMLFile, (float)(0.1*Params_.WCSAxesLength), 1.0f, 1.0f, 1.0f, 1.0,
                     (float)Params_.WCSAxesLength, "WorldCoordinateSystem");
}

// ---------------------------- OPENGL functions ----------------------------
#ifdef BIAS_HAVE_OPENGL

void ThreeDOut::OpenGLOutWCS() {

    StatekeeperGL state;

  if (Params_.WCSAxesLength<=0.0) return;

  // stolen from geometrygl

  glDisable(GL_TEXTURE_2D);
  glDisable(GL_LIGHTING);
  
  float linewidth = 0.0;
  glGetFloatv(GL_LINE_WIDTH,&linewidth);
    
  if ((linewidth>=1.0) && (linewidth<32.0))
    glLineWidth((GLfloat)linewidth);
  else
    glLineWidth((GLfloat)1.0);

#if defined(BIAS_HAVE_OPENGL) && defined(BIAS_HAVE_FONTGL)
  BIAS::DrawTextGL drawer;
  bool drawerInited = false;
    if (drawer.InitFont(NULL) == 0){
        drawerInited = true;
    }
#endif
  // X-axis:
  glColor4d(1.0, 0.0, 0.0, 0.0); // r
  glBegin(GL_LINES);
  glVertex3d(0.0, 0.0, 0.0);
  glVertex3d(Params_.WCSAxesLength, 0.0, 0.0);
  glEnd();
#if defined(BIAS_HAVE_OPENGL) && defined(BIAS_HAVE_FONTGL)
  if(drawerInited)
    drawer.Print3D("X",Params_.WCSAxesLength,0,0);
#endif

  // Y-axis:
  glColor4d(0.0, 1.0, 0.0, 0.0); // g
  glBegin(GL_LINES);
  glVertex3d(0.0, 0.0, 0.0);
  glVertex3d(0.0, Params_.WCSAxesLength, 0.0);
  glEnd();
#if defined(BIAS_HAVE_OPENGL) && defined(BIAS_HAVE_FONTGL)
  if(drawerInited)
    drawer.Print3D("Y",0,Params_.WCSAxesLength,0);
#endif 
  // Z-axis:
  glColor4d(0.0, 0.0, 1.0, 0.0); // r
  glBegin(GL_LINES);
  glVertex3d(0.0, 0.0, 0.0);
  glVertex3d(0.0, 0.0, Params_.WCSAxesLength);
  glEnd();
#if defined(BIAS_HAVE_OPENGL) && defined(BIAS_HAVE_FONTGL)
  if(drawerInited)  
    drawer.Print3D("Z",0,0,Params_.WCSAxesLength);
#endif 
}

void ThreeDOut::OpenGLOutPoints(const BIAS::RGBAuc &Color)
{
    StatekeeperGL state;

  if (Params_.PointSize<1) return;
  glColor4ubv(Color.GetData());
  glPointSize((float)Params_.PointSize);
  map<unsigned int, Point3DData>::iterator it;
  switch (Params_.PointStyle) {
  case Point:
    glBegin(GL_POINTS);
    for (it=mapPoints_.begin(); it!=mapPoints_.end(); it++){
      glVertex3dv(it->second.Point.GetData());
    }
    glEnd();
    break;
  case PointSprite:
    BIASERR("PointSprites not yet implemented");
    break;
  case Sphere:
    for (it=mapPoints_.begin(); it!=mapPoints_.end(); it++){
      Vector3<double> C = it->second.Point;
      glTranslated(C[0],C[1], C[2]);
      //glColor4ubv(it->second.Color.GetData());
      DrawSphere_((double)Params_.PointSize,50, 50);
      glTranslated(-C[0], -C[1], -C[2]);
    }
    break;
  case Box:
    BIASERR("Points as Box not yet implemented");
    break;
  }
}

void ThreeDOut::OpenGLOutPoints(){

    StatekeeperGL state;

  if (Params_.PointSize<1) return;
  map<unsigned int, Point3DData>::iterator it;
  glPointSize((float)Params_.PointSize);

  //   BIAS::TimeMeasure timer;
  //   double es=0.0;
  //   std::vector<Vector4<unsigned char> > colors;
  //   std::vector<Vector3<double> > pos;

  //   vector<Vector4<unsigned char> >::iterator itC;
  //   vector<Vector3<double> >::iterator itP;
  //   for (it=mapPoints_.begin(); it!=mapPoints_.end(); it++){
  //     colors.push_back(it->second.Color);
  //     pos.push_back(it->second.Point);
  //   }


  //  cout<<"ThreeDOut Drawing :"<<mapPoints_.size()<<" elements"<<endl;
  switch (Params_.PointStyle) {
  case Point:

    //   timer.Start();

    glBegin(GL_POINTS);
    for (it=mapPoints_.begin(); it!=mapPoints_.end(); it++){
      glColor4ubv(it->second.Color.GetData());
      glVertex3dv(it->second.Point.GetData());
    }
    glEnd();

    //     glBegin(GL_POINTS);
    //     for (itC=colors.begin(),itP=pos.begin();
    //          itC!=colors.end() && itP!=pos.end();
    //          itC++,itP++){
    //       glColor4ubv(itC->GetData());
    //       glVertex3dv(itP->GetData());
    //     }
    //     glEnd();

    //     timer.Stop();
    //     meanDraw.push_back(timer.GetRealTime());

    //     for(unsigned int i=0;i<meanDraw.size();i++)
    //       es+=meanDraw[i];
    //     es/=meanDraw.size();
    //     cout<<"ThreeDOut Draw mean:\t"<<es<<endl;
    break;
  case PointSprite:
    BIASERR("PointSprites not yet implemented");
    break;
  case Sphere:
    for (it=mapPoints_.begin(); it!=mapPoints_.end(); it++){
      Vector3<double> C = it->second.Point;
      glTranslated(C[0],C[1], C[2]);
      glColor4ubv(it->second.Color.GetData());
      DrawSphere_((double)Params_.PointSize,50, 50);
      glTranslated(-C[0], -C[1], -C[2]);
    }
    break;
  case Box:
    BIASERR("Points as Box not yet implemented");
    break;
  }
}

void ThreeDOut::OpenGLOutPMatrices_(const BIAS::RGBAuc &Color, bool UseOneColor)
{
  StatekeeperGL state;

  // no cameras desired ???
  if (Params_.CameraScale<=0.0) return;

  unsigned int index=0;

  switch (Params_.CameraStyle){
  case PyramidMeshWithArrow:
  case PyramidMesh:
  case PyramidWithCenterBox:
    {
      if (Params_.LineWidth < 1) break;
      GLenum cap = GL_LINE_LOOP;

      glDisable(GL_TEXTURE_2D);
      if (UseOneColor)
        glColor4ubv(Color.GetData());

      if((float)Params_.LineWidth >= 1)
        glLineWidth((float)Params_.LineWidth);

      map<unsigned int, PMatrix3DData>::iterator itP;
      for (itP=mapPMatrices_.begin(); itP!=mapPMatrices_.end(); itP++){
        const PMatrix3DData *it = &(itP->second);

        glPushMatrix();
        glTranslated(it->C[0], it->C[1], it->C[2]);
        glScaled(Params_.CameraScale, Params_.CameraScale, Params_.CameraScale);
        glTranslated(-it->C[0], -it->C[1], -it->C[2]);

        {
          if (!UseOneColor)
            glColor4ubv(it->Color.GetData());
          glBegin( cap );
          glVertex3dv(it->C.GetData());
          glVertex3dv(it->UR.GetData());
          glVertex3dv(it->UL.GetData());
          glEnd();

          glBegin( cap );
          glVertex3dv(it->C.GetData());
          glVertex3dv(it->LR.GetData());
          glVertex3dv(it->UR.GetData());
          glEnd();

          glBegin( cap );
          glVertex3dv(it->C.GetData());
          glVertex3dv(it->LL.GetData());
          glVertex3dv(it->LR.GetData());
          glEnd();

          glBegin( cap );
          glVertex3dv(it->C.GetData());
          glVertex3dv(it->UL.GetData());
          glVertex3dv(it->LL.GetData());
          glEnd();
        }
        glPopName();
        glPopMatrix();
        index++;
      }
    }
    // now we drew the mesh, may be we should proceed with the arrow?
    if (Params_.CameraStyle==PyramidMesh)  break;
  case Arrow:

    {
      if (Params_.LineWidth < 1) break;
      map<unsigned int, PMatrix3DData>::iterator itP;
      if (UseOneColor)
        glColor4ubv(Color.GetData());
      for (itP=mapPMatrices_.begin(); itP!=mapPMatrices_.end(); itP++){
        const PMatrix3DData *it = &(itP->second);

        glPushMatrix();
        glTranslated(it->C[0], it->C[1], it->C[2]);
        glScaled(Params_.CameraScale, Params_.CameraScale, Params_.CameraScale);
        glTranslated(-it->C[0], -it->C[1], -it->C[2]);

        glPushName(BIAS_SELECTION_OFFSET_PMATRIX +index);
        {
            if (!UseOneColor)
              glColor4ubv(it->Color.GetData());
            glBegin(GL_POINTS);
            glVertex3dv(it->C.GetData());
            glEnd();
            if (Params_.CameraStyle==PyramidMeshWithArrow
                    ||Params_.CameraStyle==Arrow) {
                //glColor3f(1.0,1.0,0.0);
                glBegin(GL_LINES);
                glVertex3dv(it->C.GetData() );
                glVertex3dv(it->PP.GetData());
                glVertex3dv(it->C.GetData() );
                glVertex3dv( ((it->Up) +(it->C)).GetData() );
                glEnd();
            }
        }
        glPopName();
        glPopMatrix();
        index++;
      }
    }
    break;
  case PyramidSolid:
    {
      float mat_diffuse[] =  {Color[0]*0.7f, Color[1]*0.7f, Color[2]*0.7f, 1.0f};
      float mat_ambient[] =  {Color[0]*0.1f, Color[1]*0.1f, Color[2]*0.1f, 1.0f};
      float mat_shininess[] =  {8.0f };
      Vector3<double> normal;

      const GLenum cap = GL_POLYGON;
      glShadeModel(GL_FLAT);
      glFrontFace(GL_CW);
      glCullFace(GL_BACK);
      glEnable(GL_CULL_FACE);
      glEnable(GL_NORMALIZE);
      glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS,mat_shininess);
      if (UseOneColor)
        glColor4ubv(Color.GetData());
      map<unsigned int, PMatrix3DData>::iterator itP;
      for (itP=mapPMatrices_.begin(); itP!=mapPMatrices_.end(); itP++){
        const PMatrix3DData *it = &(itP->second);

        glPushMatrix();
        glTranslated(it->C[0], it->C[1], it->C[2]);
        glScaled(Params_.CameraScale, Params_.CameraScale, Params_.CameraScale);
        glTranslated(-it->C[0], -it->C[1], -it->C[2]);

        glPushName(BIAS_SELECTION_OFFSET_PMATRIX +index);
        {
            if (!UseOneColor)
                glColor4ubv(it->Color.GetData());

          mat_diffuse[0] = Color[0]/255.0f*0.9f;
          mat_diffuse[1] = Color[1]/255.0f*0.9f;
          mat_diffuse[2] = Color[2]/255.0f*0.9f;
          mat_diffuse[3] = 1.0f;

          mat_ambient[0] = Color[0]/255.0f*0.2f;
          mat_ambient[1] = Color[1]/255.0f*0.2f;
          mat_ambient[2] = Color[2]/255.0f*0.2f;
          mat_ambient[3] = 1.0f;

          glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuse);
          glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);

          glBegin( cap );
          (it->C - it->UL).CrossProduct(it->C - it->UR, normal);
          glNormal3f((float)normal[0], (float)normal[1], (float)normal[2]);
          glVertex3dv(it->C.GetData());
          glVertex3dv(it->UL.GetData());
          glVertex3dv(it->UR.GetData());
          glEnd();

          glBegin( cap );
          (it->C - it->UR).CrossProduct(it->C - it->LR, normal);
          glNormal3f(-(float)normal[0], -(float)normal[1], -(float)normal[2]);
          glVertex3dv(it->C.GetData());
          glVertex3dv(it->UR.GetData());
          glVertex3dv(it->LR.GetData());
          glEnd();

          glBegin( cap );
          (it->C - it->LR).CrossProduct(it->C - it->LL, normal);
          glNormal3f(-(float)normal[0], -(float)normal[1], -(float)normal[2]);
          glVertex3dv(it->C.GetData());
          glVertex3dv(it->LR.GetData());
          glVertex3dv(it->LL.GetData());
          glEnd();

          glBegin( cap );
          (it->C - it->LL).CrossProduct(it->C - it->UL, normal);
          glNormal3f((float)normal[0], (float)normal[1], (float)normal[2]);
          glVertex3dv(it->C.GetData());
          glVertex3dv(it->LL.GetData());
          glVertex3dv(it->UL.GetData());
          glEnd();

          // camera plane
          glBegin( cap );
          (it->C - it->LR).CrossProduct(it->C - it->UR, normal);
          glNormal3f((float)normal[0], (float)normal[1], (float)normal[2]);
          glVertex3dv(it->UL.GetData());
          glVertex3dv(it->LR.GetData());
          glVertex3dv(it->UR.GetData());
          glVertex3dv(it->UL.GetData());
          glVertex3dv(it->LL.GetData());
          glVertex3dv(it->LR.GetData());
          glEnd();
        }
        glPopName();
        glPopMatrix();
        index++;
      }
    }
    break;
  case Undrawn:
    break;
  default:
    BIASERR("unknown camera drawing style "<<Params_.CameraStyle);
    break;
  }

  //draw camera covariance ellipses
  if (Params_.DrawCameraUncertaintyEllipsoids){
    GLUquadric *myQuad;
    myQuad = gluNewQuadric();
    map<unsigned int, Ellipsoid3DData>::iterator it;
    if (UseOneColor)
        glColor4ubv(Color.GetData());
    for (it=mapCamEllipsoids_.begin(); it!=mapCamEllipsoids_.end(); it++){
      glPushMatrix();
      // move to right location
      glTranslated(it->second.C[0], it->second.C[1], it->second.C[2]);
      glRotated(it->second.dAngle * DEGREE_PER_RAD, // gl wants degree
        it->second.RotAxis[0], it->second.RotAxis[1],
        it->second.RotAxis[2]);
      glScaled(Params_.EllipsoidScale * it->second.s0,
        Params_.EllipsoidScale * it->second.s1,
        Params_.EllipsoidScale * it->second.s2);

      if (!UseOneColor)
        // gl wants opacity not transparency
        glColor4ubv(it->second.Color.GetData());
      gluSphere(myQuad, 1, 20, 20);
      glPopMatrix();
    }
    gluDeleteQuadric(myQuad);
  }

}

void ThreeDOut::OpenGLOutPMatrices(const BIAS::RGBAuc &Color){
  OpenGLOutPMatrices_(Color, true);
}

void ThreeDOut::OpenGLOutPMatrices()
{
  BIAS::RGBAuc Color;
  OpenGLOutPMatrices_(Color, false);
}

void ThreeDOut::OpenGLOutImages(const bool & disableBlending)
{
  map<unsigned int, Image3DData>::iterator it=mapImages_.begin();
  // nothing to do ?
  if (it==mapImages_.end()) return;

  StatekeeperGL state(GL_ENABLE_BIT | GL_CURRENT_BIT | GL_TEXTURE_BIT);

  GLuint texid;
  glGenTextures( 1, &texid ) ;
  glBindTexture( GL_TEXTURE_2D, texid) ;
  glEnable(GL_TEXTURE_2D);
  glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);
  glDisable(GL_TEXTURE_GEN_S);
  glDisable(GL_TEXTURE_GEN_T);

  /// \todo image interface (rgb<=>grey)
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);

  // no transparency?
  if (disableBlending)
    glDisable(GL_BLEND);

  for (it=mapImages_.begin(); it!=mapImages_.end(); it++)
  {
    // use roi to determine texture coordinates
    double texX(1.0), texY(1.0);
    unsigned int x1, x2, y1, y2;
    it->second.ImageData.GetROI()->GetCorners(x1, y1, x2, y2);
    texX = (double)x2 / (double)it->second.ImageData.GetWidth();
    texY = (double)y2 / (double)it->second.ImageData.GetHeight();

    GLenum GLColorModel =
      (it->second.ImageData.GetChannelCount()==3)? GL_RGB : GL_LUMINANCE;

    glTexImage2D(GL_TEXTURE_2D, 0, GLColorModel,
                 it->second.ImageData.GetWidth(),
                 it->second.ImageData.GetHeight(), 0, GLColorModel,
                 GL_UNSIGNED_BYTE,
                 (GLvoid *) it->second.ImageData.GetImageData());

    glBegin(GL_QUADS);

    glTexCoord2d(0.0, 0.0);
    glVertex3dv(it->second.UL.GetData());

    glTexCoord2d(0.0, texY);
    glVertex3dv(it->second.LL.GetData());

    glTexCoord2d(texX, texY);
    glVertex3dv(it->second.LR.GetData());

    glTexCoord2d(texX, 0.0);
    glVertex3dv(it->second.UR.GetData());

    glEnd();
  }
  glDeleteTextures(1, &texid);
}

void ThreeDOut::OpenGLOutIndexedFaceSets() {

    StatekeeperGL state;

  map<unsigned int, IndexedFaceSet>::iterator it;
  for (it = mapIndexedFaceSet_.begin(); it!=mapIndexedFaceSet_.end(); it++) {

    if(it->second.compiled_) {
      glCallList(it->second.displayList_);
    }
    else
    {
      if (it->second.useTexture) {
        Image<unsigned char> flippedImage = it->second.texture;
        if (flippedImage.IsEmpty()) {
          flippedImage.Init(16,16,3);
          flippedImage.SetColorModel(ImageBase::CM_RGB);
        } else {
          flippedImage.Flip();
          flippedImage.PadToPowerOfTwo(0);
        }

        //      cout<<"compiling display list ..."<<endl;
        glDeleteTextures(1, &it->second.textureObjectID_);
        glGenTextures(1, &it->second.textureObjectID_);
        glBindTexture( GL_TEXTURE_2D, it->second.textureObjectID_);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP);
        glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);


        unsigned int width, height;
        width= flippedImage.GetWidth();
        height= flippedImage.GetHeight();

        GLenum textureFormat = GL_RGB;
        int channels = it->second.texture.GetChannelCount();
        //cout<<"Channels:"<<channels<<endl;
        if (channels==1) {
            //cout<<"grey texture !"<<endl;
          textureFormat = GL_LUMINANCE;
          BIASASSERT(it->second.texture.GetColorModel()==ImageBase::CM_Grey);
        } else if (channels==3) {
          textureFormat = GL_RGB;
          //         cout<<"rgb texture !"<<endl;
          BIASASSERT(it->second.texture.GetColorModel()==ImageBase::CM_RGB);
        } else if (channels==4) {
          textureFormat = GL_RGBA;
          BIASASSERT(it->second.texture.GetColorModel()==ImageBase::CM_RGBA);
          glEnable (GL_BLEND);
          glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
        } else {
          BIASERR("unsupported color model.");
          BIASABORT;
        }

#ifdef BIAS_HAVE_GLEW
        if(GLEW_EXT_framebuffer_object) {
          glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,
                          GL_LINEAR_MIPMAP_LINEAR);
          glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height,
                       0, textureFormat, GL_UNSIGNED_BYTE, 0);
          glTexParameteri(GL_TEXTURE_2D,GL_GENERATE_MIPMAP, GL_TRUE);
        } else {
          glTexParameteri(GL_TEXTURE_2D,
                          GL_TEXTURE_MIN_FILTER, GL_LINEAR);
        }
#else
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
#endif

        glTexImage2D(GL_TEXTURE_2D, 0, textureFormat, width, height,
                     0, textureFormat, GL_UNSIGNED_BYTE,
                     flippedImage.GetImageData());

        glDeleteLists(it->second.displayList_, 1);
        it->second.displayList_ = glGenLists(1);

        glNewList(it->second.displayList_, GL_COMPILE_AND_EXECUTE);
        glPushAttrib(GL_TEXTURE_BIT | GL_ENABLE_BIT);
        glPushClientAttrib(GL_CLIENT_ALL_ATTRIB_BITS);

        glVertexPointer(3, GL_FLOAT, 0, it->second.VertArray);
        glTexCoordPointer(2, GL_FLOAT, 0, it->second.TextureCoordArray);

        if(it->second.useNormals)
          glNormalPointer(GL_FLOAT, 0, it->second.NormalArray);

        glEnable(GL_TEXTURE_2D);
        if(it->second.useNormals)
            glEnableClientState(GL_NORMAL_ARRAY);
        glEnableClientState(GL_VERTEX_ARRAY);
        glEnableClientState(GL_TEXTURE_COORD_ARRAY);
        //render indexed face set
        glBindTexture( GL_TEXTURE_2D, it->second.textureObjectID_);
      }//end if (it->second.useTexture)
      else if(it->second.useColoredVertices){
        //cout<<"Using colored vertices"<<endl;;
        glDeleteLists(it->second.displayList_, 1);
        it->second.displayList_ = glGenLists(1);

        glNewList(it->second.displayList_, GL_COMPILE_AND_EXECUTE);
        glPushClientAttrib(GL_CLIENT_ALL_ATTRIB_BITS);

        glVertexPointer(3, GL_FLOAT, 0, it->second.VertArray);
        if(it->second.useNormals)
          glNormalPointer(GL_FLOAT, 0, it->second.NormalArray);
        glColorPointer(4, GL_UNSIGNED_BYTE, 0, it->second.ColorArray);
        glDisable(GL_TEXTURE_2D);
        glEnableClientState(GL_VERTEX_ARRAY);
        if(it->second.useNormals)
            glEnableClientState(GL_NORMAL_ARRAY);
        glEnableClientState(GL_COLOR_ARRAY);

        glEnable( GL_COLOR_MATERIAL );
        glColorMaterial( GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE );
      }//end if(it->second.useColoredVertices){
      else {
        glDeleteLists(it->second.displayList_, 1);
        it->second.displayList_ = glGenLists(1);

        glNewList(it->second.displayList_, GL_COMPILE_AND_EXECUTE);
        glPushClientAttrib(GL_CLIENT_ALL_ATTRIB_BITS);

        glVertexPointer(3, GL_FLOAT, 0, it->second.VertArray);
        if(it->second.useNormals)
          glNormalPointer(GL_FLOAT, 0, it->second.NormalArray);

        glDisable(GL_TEXTURE_2D);
        glEnableClientState(GL_VERTEX_ARRAY);
        if(it->second.useNormals)
            glEnableClientState(GL_NORMAL_ARRAY);


        float white[4] = {0.8, 0.8, 0.8, 1.0};
        glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, white);
      }//end else  (it->second.useTexture) | it->second.useColoredVertices)

#ifdef BIAS_HAVE_GLEW
      if (GLEW_VERSION_1_4) {
        /*
        cout<<"it->second.NumFacets:"<<it->second.NumFacets<<endl;
              for(int i=0;i<it->second.NumFacets;i++){
                if(isinf(it->second.facetIndicesCountArray[i])){ 
                    it->second.facetIndicesCountArray[i]=0;
                    cout<<"C:INF"<<endl;
                }
                else cout<<"NC:"<<it->second.facetIndicesCountArray[i]<<endl;
                    
                for(int j=0;j<it->second.facetIndicesCountArray[i];j++){
                    if(isinf(it->second.facetArray[i][j])){
                        it->second.facetArray[i][j]=0;
                        cout<<"AR:INF"<<endl;
                    }
                      else cout<<"AR:"<<it->second.facetArray[i][j];
                }
                  cout<<endl;
              }
             */
              glMultiDrawElements(Params_.IndexedFaceSetPrimitiveMode,
                    it->second.facetIndicesCountArray,
                    GL_UNSIGNED_INT,
                    (const GLvoid**)it->second.facetArray,
                    it->second.NumFacets);
      }//end if (GLEW_VERSION_1_4)
      else {
#endif
       for(int i=0; i<it->second.NumFacets;i++) {
          glDrawElements(Params_.IndexedFaceSetPrimitiveMode,
            it->second.facetIndicesCountArray[i],
            GL_UNSIGNED_INT,
            (const GLvoid*)it->second.facetArray[i]);
       }//end for
#ifdef BIAS_HAVE_GLEW
      }//end else
#endif
      if (it->second.useTexture) {
        glPopClientAttrib();
        glPopAttrib();
      } else {
        glPopClientAttrib();
      }

      //      CHECK_GL_ERROR;
      glEndList();
      it->second.compiled_=true;
    }
  }
}


void ThreeDOut::OpenGLOutLines(const BIAS::RGBAuc &Color){

  if (Params_.LineWidth < 1) return;
  StatekeeperGL state(GL_ENABLE_BIT);

  const GLenum cap = GL_LINE_LOOP;
  glDisable(GL_TEXTURE_2D);
  glColor4ubv(Color.GetData()); // same color for alls
  glLineWidth((float)Params_.LineWidth);
  map<unsigned int, Line3DData>::iterator it;
  for (it=mapLines_.begin(); it!=mapLines_.end(); it++){
    glBegin( cap );
    glVertex3dv(it->second.Start.GetData());
    glVertex3dv(it->second.End.GetData());
    glEnd();
  }
}

void ThreeDOut::OpenGLOutLines(){
  if (Params_.LineWidth < 1) return;
  StatekeeperGL state(GL_ENABLE_BIT);

  const GLenum cap = GL_LINE_LOOP;
  glDisable(GL_TEXTURE_2D);
  map<unsigned int, Line3DData>::iterator it;
  for (it=mapLines_.begin(); it!=mapLines_.end(); it++){
    // a different colro for each
    glColor4ubv(it->second.Color.GetData());

    glBegin( cap );
    glVertex3dv(it->second.Start.GetData());
    glVertex3dv(it->second.End.GetData());
    glEnd();
  }
}



void ThreeDOut::OpenGLOutEllipsoids(const BIAS::RGBAuc & Color)
{
    StatekeeperGL state;

  glEnable (GL_BLEND);
  glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  GLUquadric *myQuad;
  myQuad = gluNewQuadric();
  map<unsigned int, Ellipsoid3DData>::iterator it;
  for (it=mapEllipsoids_.begin(); it!=mapEllipsoids_.end(); it++){
    glPushMatrix();
    // move to right location
    glTranslated(it->second.C[0], it->second.C[1], it->second.C[2]);
    glRotated(it->second.dAngle * DEGREE_PER_RAD, // gl wants degree
      it->second.RotAxis[0], it->second.RotAxis[1],
      it->second.RotAxis[2]);
    glScaled(Params_.EllipsoidScale * it->second.s0,
      Params_.EllipsoidScale * it->second.s1,
      Params_.EllipsoidScale * it->second.s2);
    // gl wants opacity not transparency
    glColor4ubv(Color.GetData());
    gluSphere(myQuad, 1, 20, 20);
    glPopMatrix();
  }
  gluDeleteQuadric(myQuad);

}



void ThreeDOut::OpenGLOutEllipsoids(){
    StatekeeperGL state;

  GLUquadric *myQuad;
  myQuad = gluNewQuadric();

  // get me some lighting!
  float white[] = {1.0, 1.0, 1.0, 1.0};
  //  float red[] = {1.0, 0.0, 0.0, 1.0};
  float lightRed[] = {0.6, 0.0, 0.0, 1.0};
  //float green[] = {0.0, 1.0, 0.0, 1.0};
  //float lightGreen[] = {0.0, 0.6, 0.0, 1.0};
  float darkGreen[] = {0.0, 0.2, 0.0, 1.0};
  //float blue[] = {0.0, 0.0, 1.0, 1.0};
  float lightBlue[] = {0.0, 0.0, 0.6, 1.0};
  //float darkBlue[] = {0.0, 0.0, 0.2, 1.0};
  //float black[] = {0.0, 0.0, 0.0, 0.0};
  float darkGrey[] = {0.2, 0.2, 0.2, 1.0};
  //  float lightGrey[] = {0.6, 0.6, 0.6, 1.0};

  float position1_light[]= {1000.0, 1000.0, 3000.0, 1.0};
  glLightfv(GL_LIGHT4, GL_POSITION, position1_light);
  float position2_light[]= {-1000.0, -1000.0, 5000.0, 1.0};
  glLightfv(GL_LIGHT5, GL_POSITION, position2_light);
  float position3_light[]= {.0, .0, 4000.0, 1.0};
  glLightfv(GL_LIGHT6, GL_POSITION, position3_light);

  GLboolean hadLight[8][1];
  glGetBooleanv(GL_LIGHTING,hadLight[0]);
  glGetBooleanv(GL_LIGHT0,hadLight[1]);
  glGetBooleanv(GL_LIGHT1,hadLight[2]);
  glGetBooleanv(GL_LIGHT2,hadLight[3]);
  glGetBooleanv(GL_LIGHT3,hadLight[4]);
  glGetBooleanv(GL_LIGHT4,hadLight[5]);
  glGetBooleanv(GL_LIGHT5,hadLight[6]);
  glGetBooleanv(GL_LIGHT6,hadLight[7]);


  if(!hadLight[0][0])  glEnable(GL_LIGHTING);
  if( hadLight[1][0])  glDisable(GL_LIGHT0);
  if( hadLight[2][0])  glDisable(GL_LIGHT1);
  if( hadLight[3][0])  glDisable(GL_LIGHT2);
  if( hadLight[4][0])  glDisable(GL_LIGHT3);
  if(!hadLight[5][0])  glEnable(GL_LIGHT4);
  if(!hadLight[6][0])  glEnable(GL_LIGHT5);
  if(!hadLight[7][0])  glEnable(GL_LIGHT6);

  glLightfv(GL_LIGHT4, GL_DIFFUSE, lightBlue);
  glLightfv(GL_LIGHT4, GL_SPECULAR, white);
  glLightfv(GL_LIGHT4, GL_AMBIENT, darkGrey);
  glLightfv(GL_LIGHT5, GL_DIFFUSE, lightRed);
  glLightfv(GL_LIGHT5, GL_SPECULAR, white);
  glLightfv(GL_LIGHT5, GL_AMBIENT, darkGrey);
  glLightfv(GL_LIGHT6, GL_DIFFUSE, darkGreen);
  glLightfv(GL_LIGHT6, GL_SPECULAR, white);
  glLightfv(GL_LIGHT6, GL_AMBIENT, darkGrey);

  glFrontFace(GL_CW);
  glCullFace(GL_BACK);
  glEnable(GL_CULL_FACE);
  glEnable(GL_NORMALIZE);

  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, darkGrey);
  glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 50.0);

  glLightModelf(GL_LIGHT_MODEL_LOCAL_VIEWER,GL_TRUE);
  glShadeModel(GL_SMOOTH);
  CHECK_GL_ERROR;
  map<unsigned int, Ellipsoid3DData>::iterator it;
  unsigned char *color;     float cf[4];
  for (it=mapEllipsoids_.begin(); it!=mapEllipsoids_.end(); it++){

    color = it->second.Color.GetData();
    for(int i=0;i<4;i++) cf[i] = (float)(color[i])/255.0;

    glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, cf);
    glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR,cf);

    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    // move to right location
    glTranslated(it->second.C[0], it->second.C[1], it->second.C[2]);
    glRotated(it->second.dAngle * DEGREE_PER_RAD, // gl wants degree
      it->second.RotAxis[0], it->second.RotAxis[1],
      it->second.RotAxis[2]);
    glScaled(Params_.EllipsoidScale * it->second.s0,
      Params_.EllipsoidScale * it->second.s1,
      Params_.EllipsoidScale * it->second.s2);
    // gl wants opacity not transparency
    glColor4ubv(it->second.Color.GetData());


    gluSphere(myQuad, 1, 20, 20);
    glPopMatrix();

  }
  //turn off again
  if(!hadLight[0][0])   glDisable(GL_LIGHTING);
  if( hadLight[1][0])  glEnable(GL_LIGHT0);
  if( hadLight[2][0])  glEnable(GL_LIGHT1);
  if( hadLight[3][0])  glEnable(GL_LIGHT2);
  if( hadLight[4][0])  glEnable(GL_LIGHT3);
  if(!hadLight[5][0])  glDisable(GL_LIGHT4);
  if(!hadLight[6][0])  glDisable(GL_LIGHT5);
  if(!hadLight[7][0])  glDisable(GL_LIGHT6);

  gluDeleteQuadric(myQuad);
}

void ThreeDOut::OpenGLOut(const BIAS::RGBAuc &Color){
    if (Params_.DrawUncertaintyEllipsoids)
        OpenGLOutEllipsoids(Color);
    // no color arg:
    OpenGLOutImages();
  // override color:
  OpenGLOutPMatrices(Color);
  OpenGLOutPoints(Color);
  OpenGLOutLines(Color);
  OpenGLOutIndexedFaceSets();
  if (Params_.WCSAxesLength>0.0)
    OpenGLOutWCS();
}


void ThreeDOut::OpenGLOut(){
    BIAS::RGBAuc  Color;
    if (Params_.DrawUncertaintyEllipsoids)
        OpenGLOutEllipsoids(Color);
    OpenGLOutImages();
  OpenGLOutPMatrices();
  OpenGLOutPoints();
  OpenGLOutLines();
  OpenGLOutIndexedFaceSets();
  if (Params_.WCSAxesLength>0.0)
    OpenGLOutWCS();
}
#endif // BIAS_HAVE_OPENGL



void ThreeDOut::
DrawSphere_(double radius, int lats, int longs) {
#ifdef BIAS_HAVE_OPENGL
  int i, j;
  for(i = 0; i <= lats; i++) {
    double lat0 = M_PI * (-0.5 + (double) (i - 1) / lats);
    double z0  = sin(lat0);
    double zr0 =  cos(lat0);

    double lat1 = M_PI * (-0.5 + (double) i / lats);
    double z1 = sin(lat1);
    double zr1 = cos(lat1);

    glBegin(GL_QUAD_STRIP);
    for(j = 0; j <= longs; j++) {
      double lng = 2 * M_PI * (double) (j - 1) / longs;
      double x = cos(lng);
      double y = sin(lng);

      glNormal3f(x * zr0, y * zr0, z0);
      glVertex3f(x * zr0, y * zr0, z0);
      glNormal3f(x * zr1, y * zr1, z1);
      glVertex3f(x * zr1, y * zr1, z1);
    }
    glEnd();
  }
#endif
}


// --------------------------- VRML functions ---------------------------

int ThreeDOut::VRMLOutPoints(ostream& VRMLFile, const BIAS::RGBAuc &Color){
  if (mapPoints_.size()==0) return 0;
  map<unsigned int, Point3DData>::iterator it = mapPoints_.begin();
  while (it!=mapPoints_.end()) {
    // this not really correct because it changes the color of the
    // points and does not only change their visualization
    it->second.Color = Color;
    it++;
  }
  return VRMLOutPoints(VRMLFile);
}

int ThreeDOut::VRMLOutPointsAsPointSet_(ostream &VRMLFile)
{

  unsigned int numpoints = mapPoints_.size();
  unsigned int numpointsets = numpoints /5000;
  unsigned int remainingpoints = numpoints % 5000;

  if (remainingpoints >0) numpointsets++;
  //cout <<"numpoints: "<<numpoints<<endl;
  //cout <<"numpointsets: "<<numpointsets<<endl;
  //cout <<"remainingpoints: "<<remainingpoints<<endl;

  map<unsigned int, Point3DData>::iterator it1 = mapPoints_.begin();
  map<unsigned int, Point3DData>::iterator it2 = mapPoints_.begin();

  for (unsigned int ps=0;ps<numpointsets; ps++)
  {
    VRMLFile<<
      "Shape { "<<endl<<
      " geometry PointSet {"<<endl<<
      "  color Color {"<<endl<<
      "   color [ "<<endl;
    double r,g,b;
    //int p = 0;
    while (it1!=mapPoints_.end()) {// && p<5000) {
      Point3DData &Da = it1->second;
      r = (double)Da.Color[0] / 255.0;
      g = (double)Da.Color[1] / 255.0;
      b = (double)Da.Color[2] / 255.0;
      if (it1 != mapPoints_.begin()) // && (p%5000)!=0)
    VRMLFile<<", "<<endl;
      VRMLFile << "   "<< r << " " << g  << " " << b;
      it1++; //p++;
    }
    
    VRMLFile<< " ] " << endl
            << "  }"<<endl
            << "  coord Coordinate { "<<endl
            << "   point [ "<<endl;
    
    streamsize w=8;
    //p = 0;
    while (it2!=mapPoints_.end()) { //&& p<5000) {
      Point3DData &Db = it2->second;
      if (it2!=mapPoints_.begin()) // && (p%5000)!=0)
    VRMLFile<<", "<<endl;
      VRMLFile<< "   "
          << setw(w) << Db.Point[0] << " "
          << setw(w) << Db.Point[1] << " "
          << setw(w) << Db.Point[2];
      it2++; //p++;
    }

    VRMLFile << " ] " << endl
             << "  }" << endl
             << " }" << endl
             << "}" << endl;
  } // for all pointsets

  return 0;
}



int ThreeDOut::VRMLOutPointsAsBoxes_(ostream &VRMLFile)
{
  VRMLFile<<
    "Shape { "<<endl<<
    "  geometry IndexedFaceSet {"<<endl<<
    "    color Color {"<<endl<<
    "      color ["<<endl;

  double r,g,b;
  map<unsigned int, Point3DData>::iterator it = mapPoints_.begin();
  while (it!=mapPoints_.end()) {
    Point3DData &Da = it->second;
    r = (double)Da.Color[0] / 255.0;
    g = (double)Da.Color[1] / 255.0;
    b = (double)Da.Color[2] / 255.0;
    if (it!=mapPoints_.begin()) // add a "," behind last line
      VRMLFile<<","<<endl;
    for (unsigned int i=0;i<7;i++)
      VRMLFile <<
      "      "<< r << " " << g  << " " << b<<","<<endl;
    VRMLFile <<
      "      "<< r << " " << g  << " " << b;
    it++;
  }

  VRMLFile<<endl<<
    "      ]"<<endl<<
    "    }"<<endl<<
    "    coord Coordinate {"<<endl<<
    "      point ["<<endl;


  float size = float(Params_.PointSize / 2.0);
  streamsize w=8;
  it=mapPoints_.begin();
  while (it!=mapPoints_.end()) {
    Point3DData &Db = it->second;
    if (it!=mapPoints_.begin()) // add a "," behind last line
      VRMLFile<<","<<endl<<endl;
    // top four points
    VRMLFile<<
      "      "
      << setw(w) << Db.Point[0]-size << " "
      << setw(w) << Db.Point[1]+size << " "
      << setw(w) << Db.Point[2]-size;
    VRMLFile<< ","<<endl;
    VRMLFile<<
      "      "
      << setw(w) << Db.Point[0]+size << " "
      << setw(w) << Db.Point[1]+size << " "
      << setw(w) << Db.Point[2]-size;
    VRMLFile<< ","<<endl;
    VRMLFile<<
      "      "
      << setw(w) << Db.Point[0]+size << " "
      << setw(w) << Db.Point[1]+size << " "
      << setw(w) << Db.Point[2]+size;
    VRMLFile<< ","<<endl;
    VRMLFile<<
      "      "
      << setw(w) << Db.Point[0]-size << " "
      << setw(w) << Db.Point[1]+size << " "
      << setw(w) << Db.Point[2]+size;
    VRMLFile<< ","<<endl;
    // bottom four points
    VRMLFile<<
      "      "
      << setw(w) << Db.Point[0]-size << " "
      << setw(w) << Db.Point[1]-size << " "
      << setw(w) << Db.Point[2]-size;
    VRMLFile<< ","<<endl;
    VRMLFile<<
      "      "
      << setw(w) << Db.Point[0]+size << " "
      << setw(w) << Db.Point[1]-size << " "
      << setw(w) << Db.Point[2]-size;
    VRMLFile<< ","<<endl;
    VRMLFile<<
      "      "
      << setw(w) << Db.Point[0]+size << " "
      << setw(w) << Db.Point[1]-size << " "
      << setw(w) << Db.Point[2]+size;
    VRMLFile<< ","<<endl;
    VRMLFile<<
      "      "
      << setw(w) << Db.Point[0]-size << " "
      << setw(w) << Db.Point[1]-size << " "
      << setw(w) << Db.Point[2]+size;
    it++;
  }

  VRMLFile << endl<<
    "      ]" << endl<<
    "    }" << endl<< // end of Coordinate
    "    coordIndex ["<<endl;
  for (unsigned int i=0;i<mapPoints_.size();i++) {
    if (i!=0)
      VRMLFile<<","<<endl<<endl;
    VRMLFile<<"          "<<
      i*8+0<<", "<<i*8+1<<", "<<i*8+2<<", "<<i*8+3<<", -1,"<<
      " # top side"<<endl<<"          "<<
      i*8+7<<", "<<i*8+6<<", "<<i*8+5<<", "<<i*8+4<<", -1,"<<
      " # bottom side"<<endl<<"          "<<
      i*8+4<<", "<<i*8+5<<", "<<i*8+1<<", "<<i*8+0<<", -1,"<<
      " # back side"<<endl<<"          "<<
      i*8+3<<", "<<i*8+2<<", "<<i*8+6<<", "<<i*8+7<<", -1,"<<
      " # front side"<<endl<<"          "<<
      i*8+0<<", "<<i*8+3<<", "<<i*8+7<<", "<<i*8+4<<", -1,"<<
      " # left side"<<endl<<"          "<<
      i*8+2<<", "<<i*8+1<<", "<<i*8+5<<", "<<i*8+6<<", -1";
  }

  VRMLFile<<endl<<
    "     ]"<<endl<<
    "     ccw FALSE"<<endl<<
    "     colorPerVertex TRUE"<<endl<<
    "  }" << endl<<
    "}" << endl;

  return 0;
}

int ThreeDOut::VRMLOutPoints(ostream& VRMLFile){
  if (mapPoints_.size()==0) return 0;
  int res=0;
  switch (Params_.PointStyle) {
  case Point:
    res=VRMLOutPointsAsPointSet_(VRMLFile);
    break;
  case PointSprite:
    BIASERR("PointSprites not yet implemented");
    break;
  case Sphere:
    res = VRMLOutPointsAsSpheres_(VRMLFile);
    break;
  case Box:
    res = VRMLOutPointsAsBoxes_(VRMLFile);
    break;
  }
  return res;
}

int ThreeDOut::VRMLOutPMatrices(ostream& VRMLFile, const BIAS::RGBAuc &Color)
{
  int res = 0;
  switch (Params_.CameraStyle) {
  case Arrow: res = VRMLOutPMatricesAsArrows_(VRMLFile, Color); break;
  case PyramidMesh: res = VRMLOutPMatricesAsMesh_(VRMLFile, Color); break;
  default: BIASERR("Not yet implmented"); break;
  }

  if (res == 0 && Params_.DrawCameraUncertaintyEllipsoids){
    map<unsigned int, Ellipsoid3DData>::iterator it;
    for (it=mapCamEllipsoids_.begin(); it!=mapCamEllipsoids_.end(); it++) {
      Ellipsoid3DData& E = it->second;
      VRMLOutEllipsoid_(VRMLFile, E, Color);
    }
  }
  return res;
}

int ThreeDOut::VRMLOutPMatricesAsMesh_(ostream& VRMLFile,
                                       const BIAS::RGBAuc &Color)
{
  if (Params_.LineWidth < 1) return 0;
  double r = 0;
  double g = 0;
  double b = 0;
  // VRML transparency=0 is opaque
  //double transparency = 0;

  map<unsigned int, PMatrix3DData>::iterator itP;
  r =  (double)Color[0] / 255.0;
  g =  (double)Color[1] / 255.0;
  b =  (double)Color[2] / 255.0;
  //transparency = VRMLtransparency(Color);
  unsigned int num=0;
  // create vrml model of camera
  VRMLFile << "Shape {" << endl;
  // write camera center and 4 points in the image plane
  VRMLFile <<"  geometry IndexedLineSet {" << endl;
  VRMLFile <<"    coord Coordinate {"      << endl;
  VRMLFile <<"      point [ "              << endl;
  for (itP=mapPMatrices_.begin(); itP!=mapPMatrices_.end(); itP++){
    const PMatrix3DData *it = &(itP->second);
    VRMLFile
      <<"        "<<it->C[0]<<" "<<it->C[1]<<" "<<it->C[2]<< " , "<< endl
      <<"        "<<it->UL[0]<<" "<<it->UL[1]<<" "<<it->UL[2] <<" , " <<endl
      <<"        "<<it->UR[0]<<" "<<it->UR[1]<<" " <<it->UR[2]<<" , " << endl
      <<"        "<<it->LR[0]<<" "<<it->LR[1]<<" " <<it->LR[2]<<" , " << endl
      <<"        "<<it->LL[0]<<" "<<it->LL[1]<<" " <<it->LL[2]<<" , " << endl;
    num++;
  }
  VRMLFile<<" ] " << endl;
  VRMLFile <<"    }" << std::endl;
  VRMLFile << "    coordIndex [" << endl;
  for (unsigned int i=0;i<num; i++) {
    unsigned n=i*5;
    VRMLFile <<"      "<<n<<", "<<n+2<<", "<<n+1<<", -1" <<endl;
    VRMLFile <<"      "<<n<<", "<<n+1<<", "<<n+4<<", -1" <<endl;
    VRMLFile <<"      "<<n<<", "<<n+4<<", "<<n+3<<", -1" <<endl;
    VRMLFile <<"      "<<n<<", "<<n+3<<", "<<n+2<<", -1" <<endl;
  }
  VRMLFile << "       ] " << endl;
  VRMLFile << "  }" << endl;    // terminates IndexedLineSet
  // set color of lines
  VRMLFile << "  appearance Appearance { " << endl;
  VRMLFile << "    material Material {"<<endl
    << "      diffuseColor  " << " "<<r<<" "<<g<<" "<<b<<endl
    << "      emissiveColor " << " "<<r<<" "<<g<<" "<<b<<endl
    <<"     }"<< endl;   // terminates material
  VRMLFile << "  }"   << endl;  // terminates appearance
  VRMLFile << "}" << endl;      // terminates shape
  return 0;
}

int ThreeDOut::VRMLOutPMatricesAsArrows_(ostream& VRMLFile,
                                         const BIAS::RGBAuc &Color)
{
  if (Params_.LineWidth < 1) return 0;
  double r = 0;
  double g = 0;
  double b = 0;
  //double transparency = 0;
  map<unsigned int, PMatrix3DData>::iterator itP;
  r =  (double)Color[0] / 255.0;
  g =  (double)Color[1] / 255.0;
  b =  (double)Color[2] / 255.0;
  //transparency = VRMLtransparency(Color);
  unsigned int num=0;
  // first: one switch-case to write entry
  VRMLFile << "Shape {" << endl;
  VRMLFile << "   geometry IndexedLineSet {" << endl;
  VRMLFile << "      coord Coordinate {" << endl;
  VRMLFile << "         point [" << endl;
  Vector3<double> up;
  // then: for-loop with switch-case to write cams efficient
  for (itP=mapPMatrices_.begin(); itP!=mapPMatrices_.end(); itP++){
    const PMatrix3DData *it = &(itP->second);
    // write the line
    up = it->C + it->Up;
    VRMLFile
      << "      " <<up[0]<<" "<<up[1]<<" "<<up[2] <<", "<<endl
      << "   " <<it->C[0]<<" "<<it->C[1]<<" "<<it->C[2]<<","<<endl
      << "      " <<it->PP[0]<<" "<<it->PP[1]<<" "<<it->PP[2] <<", "<<endl;
    num++;
  }
  VRMLFile<< "               ]" << endl;
  VRMLFile << "      }"<<endl;
  VRMLFile << "   coordIndex [ "<<endl;
  for (unsigned int i=0;i<num; i++)
    VRMLFile <<"      "<<i*3<<"  "<<i*3+1<<"  "<<i*3+2<<"  -1"<<endl;
  VRMLFile <<" ]"<< endl;
  VRMLFile << "   color Color {"<<endl;
  VRMLFile << "        color [ "<<endl;
  for (unsigned int i=0;i<num; i++)
    VRMLFile <<r<<" "<<g<<" "<<b<<", "  <<r<<" "<<g<<" "<<b<<" ,"<<endl;
  VRMLFile << " ] "<<endl;
  VRMLFile << "   }\n}\n}"<< endl;
  return 0;
}

int ThreeDOut::VRMLOutPMatricesRoutine(ostream& VRMLFile,
                                       const PMatrix3DData *it,
                                       double r,
                                       double g,
                                       double b,
                                       double transparency,
                                       bool CenterAsSphere){
  switch (Params_.CameraStyle) {
  case PyramidSolid:
    r =  (double)it->Color[0] / 255.0;
    g =  (double)it->Color[1] / 255.0;
    b =  (double)it->Color[2] / 255.0;
    transparency =  VRMLtransparency(it->Color);

    // create vrml model of camera
    VRMLFile << std::endl;
    //       if (it->name.length()>0)
    //  VRMLFile <<"  DEF "<< it->name;
    VRMLFile << "  Shape {" << std::endl;

    // write camera center and 4 points in the image plane
    if (it->name.length()>0)
      VRMLFile <<"         geometry DEF "<<CorrectDefString_(it->name);
    else
      VRMLFile <<"         geometry";
    VRMLFile <<" IndexedFaceSet {" << std::endl;
    VRMLFile <<"            coord Coordinate {" << std::endl;
    VRMLFile <<"              point [ " << std::endl;
    VRMLFile <<"                       "<<it->C[0]<<" "<<it->C[1]<<" "
             <<it->C[2]<< " , " << std::endl;
    VRMLFile <<"                       "<<it->UL[0]<<" "<<it->UL[1]
             <<" "<<it->UL[2] <<" , " <<std:: endl;
    VRMLFile <<"                       "<<it->UR[0]<<" "<<it->UR[1]<<" "
             <<it->UR[2]<<" , " <<  std::endl;
    VRMLFile <<"                       "<<it->LR[0]<<" "<<it->LR[1]<<" "
             <<it->LR[2]<<" , " <<  std::endl;
    VRMLFile <<"                       "<<it->LL[0]<<" "<<it->LL[1]<<" "
             <<it->LL[2]  <<" ] " <<  std::endl;

    VRMLFile <<"            }" << std::endl;

    //  write triangulation for topology
    VRMLFile << "              coordIndex [" << endl;
    VRMLFile << "                           0, 2, 1, -1, " << std::endl;
    VRMLFile << "                           0, 1, 4, -1, " << std::endl;
    VRMLFile << "                           0, 4, 3, -1, " << std::endl;
    VRMLFile << "                           0, 3, 2, -1, " << std::endl;
    VRMLFile << "                           1, 2, 4, -1, " << std::endl;
    VRMLFile << "                           4, 2, 3, -1 ]" << std::endl;

    VRMLFile << "  }" << std::endl << std::endl; // terminates IndexedFaceSet

    VRMLFile << "         appearance Appearance { " << endl;

    VRMLFile << "              material Material { diffuseColor  "
             << " "<<r<<" "<<g<<" "<<b<< std::endl //terminates material
             << " transparency  "<< transparency
             <<"}"<<std::endl;
    VRMLFile << "} "   << std::endl;  // terminates appearance
    VRMLFile << "   }" << std::endl;  // terminates shape

    break;
  case PyramidMeshWithArrow:
  case PyramidMesh:
    if (Params_.LineWidth < 1) return 0;
    r =  (double)it->Color[0] / 255.0;
    g =  (double)it->Color[1] / 255.0;
    b =  (double)it->Color[2] / 255.0;

    // create vrml model of camera
    VRMLFile << endl;
    VRMLFile << "Shape {" << endl;

    // write camera center and 4 points in the image plane
    VRMLFile <<"  geometry IndexedLineSet {" << endl;
    VRMLFile <<"    coord Coordinate {"      << endl;
    VRMLFile <<"      point [ "              << endl;
    VRMLFile <<"        "<<it->C[0]<<" "<<it->C[1]<<" "
             <<it->C[2]<< " , " << endl;
    VRMLFile <<"        "<<
      it->C[0]+Params_.CameraScale*(it->UL[0]-it->C[0]) <<" "<<
      it->C[1]+Params_.CameraScale*(it->UL[1]-it->C[1]) <<" "<<
      it->C[2]+Params_.CameraScale*(it->UL[2]-it->C[2]) <<" , " <<std:: endl;
    VRMLFile <<"        "<<
      it->C[0]+Params_.CameraScale*(it->UR[0]-it->C[0])<<" "<<
      it->C[1]+Params_.CameraScale*(it->UR[1]-it->C[1])<<" "<<
      it->C[2]+Params_.CameraScale*(it->UR[2]-it->C[2])<<" , " << endl;
    VRMLFile <<"        "<<
      it->C[0]+Params_.CameraScale*(it->LR[0]-it->C[0])<<" "<<
      it->C[1]+Params_.CameraScale*(it->LR[1]-it->C[1])<<" "<<
      it->C[2]+Params_.CameraScale*(it->LR[2]-it->C[2])<<" , " << endl;
    VRMLFile <<"        "<<
      it->C[0]+Params_.CameraScale*(it->LL[0]-it->C[0])<<" "<<
      it->C[1]+Params_.CameraScale*(it->LL[1]-it->C[1])<<" " <<
      it->C[2]+Params_.CameraScale*(it->LL[2]-it->C[2])  <<" ] " << endl;
    VRMLFile <<"    }" << std::endl;

    // conect the points with lines
    VRMLFile << "    coordIndex [" << endl;
    VRMLFile << "      0, 2, 1, -1, " << endl;
    VRMLFile << "      0, 1, 4, -1, " << endl;
    VRMLFile << "      0, 4, 3, -1, " << endl;
    VRMLFile << "      0, 3, 2, -1] " << endl;
    VRMLFile << "  }" << endl;    // terminates IndexedLineSet

    // set color of lines
    VRMLFile << "  appearance Appearance { " << endl;
    VRMLFile << "    material Material {"<<endl
             << "      diffuseColor  " << " "<<r<<" "<<g<<" "<<b<<endl
             << "      emissiveColor " << " "<<r<<" "<<g<<" "<<b<<endl
             <<"     }"<< endl;   // terminates material
    VRMLFile << "  }"   << endl;  // terminates appearance
    VRMLFile << "}" << endl;      // terminates shape
    if (Params_.CameraStyle!=PyramidMeshWithArrow) break;
  case Arrow:
    r =  (double)it->Color[0] / 255.0;
    g =  (double)it->Color[1] / 255.0;
    b =  (double)it->Color[2] / 255.0;

    if (CenterAsSphere) {
      VRMLFile << "Transform {"<<endl;
      VRMLFile << "  translation "
               << it->C[0] << " " <<it->C[1] << " " <<it->C[2]
               << endl;
      VRMLFile << "  children ";
      VRMLFile <<"   Shape {"<<endl;
      VRMLFile << "    appearance Appearance { " << endl;
      VRMLFile << "     material Material { " << endl;
      VRMLFile << "       transparency      "
               << VRMLtransparency(it->Color) <<endl;
      VRMLFile << "       diffuseColor " << r << " " << g << " "<<b << endl;
      VRMLFile << "     }" << endl; // close Material
      VRMLFile << "    }" << endl; // close Appearance
      VRMLFile << "    geometry Sphere  { radius "
               << 0.1*(it->C-it->PP).NormL2()<<" }"
               << endl;
      VRMLFile << "  }" << endl; // close Shape
      VRMLFile << "}" << endl; // close transform
    } else {
      // write the point
      VRMLFile << "Shape {" << endl;
      VRMLFile << "   geometry PointSet {"<<endl;
      VRMLFile << "      coord Coordinate {"<<endl;
      VRMLFile << "         point ["<<it->C[0]<<" "<<it->C[1]<<" "<<it->C[2]
               <<"]" << endl << "      }"<<endl;
      VRMLFile << "      color Color { color [ 0 1 0, 0 1 0 ] }"<<endl;
      VRMLFile << "   }\n}"<<endl;
    }

    // write the line
    VRMLFile << "Shape {" << endl;
    VRMLFile << "   geometry IndexedLineSet {" << endl;
    VRMLFile << "      coord Coordinate {" << endl;
    VRMLFile << "         point [" << endl;
    VRMLFile << "                    " <<it->C[0]<<" "<<it->C[1]<<" "
             <<it->C[2]<<","<<endl;
    VRMLFile << "                    " <<it->PP[0]<<" "<<it->PP[1]<<" "
             <<it->PP[2] << "                ]" << endl;
    VRMLFile << "      }"<<endl;
    VRMLFile << "   coordIndex [ 0 1 -1 ]"<< endl;
    VRMLFile << "   color Color {"<<endl;
    VRMLFile << "      color [ "<<r<<" "<<g<<" "<<b<<", "
             <<r<<" "<<g<<" "<<b<<" ] "<<endl;
    VRMLFile << "   }\n}\n}"<< endl;

    break;
  default: BIASERR("Desired Camera drawing style is not implemented."
                   <<" Skipping Cameras.");
    return -1;
  }
  return 0;
}

int ThreeDOut::VRMLOutPMatrices(ostream& VRMLFile){
  double r = 0;
  double g = 0;
  double b = 0;
  double transparency = 0;
  bool CenterAsSphere = true;
  map<unsigned int, PMatrix3DData>::iterator itP;
  for (itP=mapPMatrices_.begin(); itP!=mapPMatrices_.end(); itP++){
    const PMatrix3DData *it = &(itP->second);

    if(VRMLOutPMatricesRoutine(VRMLFile,
                               it, r, g, b,
                               transparency, CenterAsSphere) != 0)
      return -1;

    if (Params_.DrawCameraUncertaintyEllipsoids){
      map<unsigned int, Ellipsoid3DData>::iterator mit;
      for (mit=mapCamEllipsoids_.begin(); mit!=mapCamEllipsoids_.end();mit++) {
        Ellipsoid3DData& E = mit->second;
        VRMLOutEllipsoid_(VRMLFile, E, it->Color);
      }
    }
  }

  return 0;
}


int ThreeDOut::VRMLOutLines(ostream& VRMLFile, const BIAS::RGBAuc &Color){
  if (Params_.LineWidth < 1) return 0;
  double r = (double)Color[0] / 255.0;
  double g = (double)Color[1] / 255.0;
  double b = (double)Color[2] / 255.0;
  // double a = (double)it->second.Color[3] / 255.0;
  switch (Params_.LineStyle) {
  case Solid:
    for (map<unsigned int, Line3DData>::iterator it=mapLines_.begin();
      it!=mapLines_.end(); it++) {
        VRMLFile << std::endl;
        VRMLFile << "  Shape {" << std::endl;
        VRMLFile <<"         geometry IndexedLineSet {" << std::endl;
        VRMLFile <<"            coord Coordinate {" << std::endl;
        VRMLFile <<"              point [ " << std::endl;
        VRMLFile <<"                       "<<it->second.Start[0]<<" "
          << it->second.Start[1]<<" " <<it->second.Start[2]<< " , "
          << std::endl;
        VRMLFile <<"                       "<<it->second.End[0]<<" "
          <<it->second.End[1] <<" "<<it->second.End[2] <<" ] "<<std::endl;

        VRMLFile <<"            }" << std::endl;
        VRMLFile << "              coordIndex [  0, 1, -1 ]" << std::endl;
        VRMLFile << "           color Color {" << std::endl;
        VRMLFile <<"              color [  "<<r<<" "<<g<<" "<<b<<" ]"<<std::endl;
        VRMLFile << "           } " <<std:: endl;   // terminates Color
        VRMLFile << "           colorPerVertex FALSE " << std::endl;
        VRMLFile << "  }" << std::endl << std::endl; // terminates IndexedLineSet
        VRMLFile << "   }" << std::endl;  // terminates shape
      }
      break;
  default: BIASERR("Unsupported LineStyle, line drawing disabled."); return -1;
  }
  return 0;
}

int ThreeDOut::VRMLOutProjection(ostream& VRMLFile)
{
  double r, g, b;
  map<unsigned int, Projection3DData>::iterator it;

  /** VIEWPORT PART, DUE TO BE IMPLEMENTED

  **/

  for(it=mapProjection3D_.begin(); it!=mapProjection3D_.end(); it++) {
    r = (double)it->second.ColorOfSelection[0] / 255.0;
    g = (double)it->second.ColorOfSelection[1] / 255.0;
    b = (double)it->second.ColorOfSelection[2] / 255.0;
    VRMLFile <<"\t Transform {"<<std::endl;
    VRMLFile <<"\t translation "
             <<it->second.C[0]<<" "<<it->second.C[1]<<" "<<it->second.C[2]
             <<std::endl;

    VRMLFile <<"\t rotation "
             <<it->second.axis[0]<<" "<<it->second.axis[1]
             <<" "<<it->second.axis[2]<<" "<<it->second.angle
             <<std::endl;
    VRMLFile <<"\t   children ["<<std::endl;
    CoordinateSystem(VRMLFile,
                     (float)it->second.radiusSelection,
                     (float)r, (float)g, (float)b, 1.0f,
                     it->second.scale,
                     it->second.identifier);

    //If we want to visualize viewports
    if(Params_.CameraStyle == PyramidMesh ||
       Params_.CameraStyle == PyramidMeshWithArrow){
      //PERSPECTIVE CAMERA
      if(it->second.type == 0){
        //View The     perspective-Camera-View, Pyramid Type
        VRMLOutViewPortPyramidal (VRMLFile,
                                  it->second.radiusSelection,
                                  r, g, b,
                                  it->second.angle,
                                  it->second.ratio,
                                  it->second.scale,
                                  it->second.identifier);
      }

      //NON-PERSPECTIVE CAMERA
      if(it->second.type == 1){
        cout<<" trying to build conical camera rep:\n! ";
        //View The non-perspective-Camera-View, Conical Type
        VRMLOutViewPortConical (VRMLFile,
                                it->second.radiusSelection,
                                r, g, b,
                                it->second.angle,
                                it->second.scale,
                                it->second.identifier);
      }
    }

    if(Params_.CameraStyle == PyramidSolid){
      //PERSPECTIVE CAMERA
      if(it->second.type == 0){
        //View The     perspective-Camera-View, Pyramid Type
        VRMLOutViewPortPyramidal (VRMLFile,
                                  it->second.radiusSelection,
                                  r, g, b,
                                  it->second.angle,
                                  it->second.ratio,
                                  it->second.scale,
                                  it->second.identifier,
                                  true);
      }

      //NON-PERSPECTIVE CAMERA
      if(it->second.type == 1){
        //View The non-perspective-Camera-View, Conical Type
        VRMLOutViewPortConical (VRMLFile,
                                it->second.radiusSelection,
                                r, g, b,
                                it->second.angle,
                                it->second.scale,
                                it->second.identifier,
                                true);
      }
    }

    VRMLFile <<"\t   ]"<<std::endl;
    VRMLFile <<"\t }"<<std::endl;
  }

  /** VIEWPORT PART, DUE TO BE IMPLEMENTED

  **/

  return 0;
}

int ThreeDOut::VRMLOutIndexedFaceSets(ostream& VRMLFile)
{
  map<unsigned int, IndexedFaceSet>::iterator it;
  for (it = mapIndexedFaceSet_.begin(); it!=mapIndexedFaceSet_.end(); it++) {
    if (it->second.vertexSet.empty()) continue;
    VRMLFile << std::endl;
    VRMLFile <<"Transform { scale 1 1 1"<<endl<<" children [ "<<endl;
    VRMLFile << "  Shape {" << std::endl;
    //define apperance

    VRMLFile << "   appearance Appearance { " << std::endl;
    if (it->second.texture.GetSize() == 0) {
      VRMLFile << "    material Material { diffuseColor 0.8 0.8 0.8 } "<<endl;
    } else {
      VRMLFile << "# enable this line and disable the texture below to see"
               << " an untextured model with normals "<<endl;
      VRMLFile << "#    material Material { diffuseColor 0.8 0.8 0.8 } "<<endl;
      //adding texture
      VRMLFile << "# enable this line to see a textured model "<<endl;
      VRMLFile << "    texture ";
      if(it->second.textureName == "") {
        VRMLFile << "PixelTexture {" << std::endl;
        //fill in PixelTexture Size and colors in hex?
        VRMLFile << "     image "
          <<it->second.texture.GetWidth()<<" "
          <<it->second.texture.GetHeight()<<" "
          <<it->second.texture.GetChannelCount()<<std::endl;
        unsigned int channelColor;
        unsigned int writeCounter = 0;
        for (int h=it->second.texture.GetHeight()-1; h>=0; h--) {
          VRMLFile << " ";
          for(unsigned int w=0; w<it->second.texture.GetWidth(); w++) {
            VRMLFile <<"0x";
            for(unsigned int c=0; c<it->second.texture.GetChannelCount(); c++) {
              channelColor= it->second.texture.PixelValue(w,h,(unsigned short)c);
              VRMLFile.flags(ios_base::hex);
              VRMLFile.fill('0');
              VRMLFile.width(2);
              VRMLFile<<channelColor;
            }
            writeCounter++;
            if(writeCounter>=8) {
              VRMLFile<<std::endl;
              writeCounter=0;
            }
            VRMLFile.flags(ios_base::dec);
            VRMLFile <<" ";
          }
          VRMLFile<<std::endl;
        }
        VRMLFile.flags(ios_base::dec);
        VRMLFile << "    } " << endl;//end PixelTexture
      }//end if textureName == ""
      else {//textureName != ""
        // ImageTexture { url "./thermae.ppm" }
        VRMLFile << "ImageTexture { url \""<<it->second.textureName;
        VRMLFile << "\" } " << endl;//end ImageTexture
        if(it->second.writeOutTexture)
          ImageIO::Save(it->second.textureName, it->second.texture);
      }
    }  // end if GetSize() == 0
    VRMLFile <<"   }" << endl;//end appearance
    //define geometry
    if (it->second.name.length()>0)
      VRMLFile << "   geometry DEF "<<CorrectDefString_(it->second.name);
    else
      VRMLFile << "   geometry";
    VRMLFile <<" IndexedFaceSet { " << std::endl;
    //write all vertex coordinates
    VRMLFile << "    coord Coordinate {" << std::endl;
    VRMLFile << "     point [ " << std::endl;
    {
      const int vnum = int(it->second.vertexSet.size());
      for(int i = 0; i < vnum; i++) {
          VRMLFile << "     "
                   << it->second.vertexSet[i][0] << " "
                   << it->second.vertexSet[i][1] << " "
                   << it->second.vertexSet[i][2]
                   << (((i+1) == vnum) ? " ] " : ", ") << std::endl;
      }
    }
    VRMLFile <<"    } " << std::endl; //closing coord

    if (it->second.texture.GetSize() != 0) {

      //write all texture coordinates
      VRMLFile << "    texCoord TextureCoordinate { " << std::endl;
      VRMLFile << "     point [ " << std::endl;
      const int vnum = int(it->second.texCoordSet.size());
      for(int i = 0; i < vnum; i++) {
          VRMLFile << "     "
                   << it->second.texCoordSet[i][0] << " "
                   << it->second.texCoordSet[i][1]
                   << (((i+1) == vnum) ? " ] " : ", ") << std::endl;
      }
      VRMLFile <<"    }" << std::endl; //closing texCoord
    } // end if GetSize() == 0
#ifdef BIAS_HAVE_OPENGL
    // for now normals are only defined if OpenGL is enabled
    if (it->second.NormalArray != NULL) {
      VRMLFile << "    normal Normal {" << std::endl
               << "     vector [ " << std::endl;
      float *pData = it->second.NormalArray;
      const int vnum = int(it->second.vertexSet.size());
      for(int i = 0; i < vnum; i++) {
        VRMLFile << "     " << *pData++;
        VRMLFile << " " << *pData++;
        VRMLFile << " " << *pData++;
        VRMLFile << (((i+1) == vnum) ? " ] " : ", ") << std::endl;
      }
      VRMLFile << "    }" << endl; //closing normal
    }
#endif
    //define faces per indices
    VRMLFile << "    coordIndex [ " << std::endl;
    {
      const int vnum = int(it->second.faceIndices.size());
      for(int i = 0; i < vnum; i++) {
        VRMLFile << "    ";
        for(int j = 0; j < (it->second.faceIndices[i]).size(); j++)
          VRMLFile << it->second.faceIndices[i][j] << ", ";
        VRMLFile << " -1 " << (((i+1) == vnum) ? " ] " : ", ") << std::endl;
      }
    }

    VRMLFile <<"   }" << std::endl;//closing IndexedFaceSet
    VRMLFile <<"  }" << std::endl;//closing SHAPE
    VRMLFile <<" ] "<<std:: endl <<"} "<<std::endl;// end tranform , end children
  }
  return 0;
}

int ThreeDOut::VRMLOutImages(ostream& VRMLFile)
{
  map<unsigned int, Image3DData>::iterator it;
  for (it = mapImages_.begin(); it!=mapImages_.end(); it++) {
    Vector3<double> Center(0.25*(it->second.UL + it->second.LR +
                                 it->second.LL + it->second.UR));

    VRMLFile << "Transform { "<<endl;

    //VRMLFile << " center "
    //       <<Center[0]<<" "<<Center[1]<<" "<<Center[2]<<endl;
    //VRMLFile << " rotation 0 1 0 1.57  # [-1,1],(-,) "<<endl;
    //VRMLFile << " scale 1 1 1    # (0,) "<<endl;
    //VRMLFile << " scaleOrientation 0 0 1 0  # [-1,1],(-,) "<<endl;

    VRMLFile << " translation "<< Center[0]<<" "<< Center[1]<<" "<< Center[2] <<endl;
    VRMLFile << " children [ "<<endl;

    if (it->second.name != "") {
      VRMLFile << "  Transform {  "<<endl;
      //VRMLFile << "   center           "
      //         <<Center[0]<<" "<<Center[1]<<" "<<Center[2]<<endl;

      VRMLFile << "   rotation 1 0 0 -1.57  # [-1,1],(-,) "<<endl;
      //VRMLFile << "   scale 1 1 1    # (0,) "<<endl;
      //VRMLFile << "   scaleOrientation 0 0 1 0  # [-1,1],(-,) "<<endl;
      //VRMLFile << "   translation "<< Center[0]<<" "<< Center[1]<<" "<<Center[2]<<endl;
      VRMLFile << "   children [ "<<endl;

      VRMLFile << "    Text { "<<endl;
      VRMLFile << "     string \"" <<it->second.name <<"\" "<<endl;
      //VRMLFile << " exposedField  SFNode   fontStyle NULL"<<endl;
      //  VRMLFile << " exposedField  MFFloat  length    []      # [0,)"<<endl;
      //  VRMLFile << " exposedField  SFFloat  maxExtent 0.0     # [0,)"<<endl;
      VRMLFile << "    }"<<endl;
      VRMLFile<<"   ]"<<endl;
      VRMLFile << "  }"<<endl;
    }

    if (it->second.BillBoard)
    {
      Vector3<double> diag1(it->second.UL - it->second.LR),
                      diag2(it->second.LL - it->second.UR);
      diag1.Normalize();
      diag2.Normalize();
      Vector3<double>
        RotAx(diag1.CrossProduct(diag2).CrossProduct(Vector3<double>(0,1,0)));
      //double angle = 0;
      if (RotAx.NormL2()>1e-8) {
        // angle = arcsin(RotAx.NormL2());
        BIASERR("Billboard must lie in xz plane !!!");
      }

      Vector3<double> AxisOfRotation1(0,0,0);
      VRMLFile << std::endl;
      VRMLFile << "Billboard { "<<endl;
      if (AxisOfRotation1.NormL2()>0.0) AxisOfRotation1.Normalize();
      VRMLFile << " axisOfRotation "<<  AxisOfRotation1[0]<<" "
               <<  AxisOfRotation1[1]<<" "<<  AxisOfRotation1[2]
               <<" " <<endl;

      VRMLFile << " children [ "<<endl;
    }

    VRMLFile << std::endl;
    VRMLFile << "  Shape { " << std::endl;
    ////////////////////////////////////////    
    VRMLFile << "   appearance Appearance { " << std::endl; 
    VRMLFile << "    material Material { "<<endl;    
    VRMLFile << "     diffuseColor   0.2 0.2 0.2    # MFColor"<<endl;
    VRMLFile << "     specularColor  0 0 0          # MFColor"<<endl;
    VRMLFile << "     emissiveColor  0.2 0.2 0.2    # MFColor"<<endl;
    VRMLFile << "     shininess      0              # MFFloat"<<endl;
    VRMLFile << "     transparency   0              # MFFloat  "<<endl;
    VRMLFile << "    } "<<endl;
    VRMLFile << "   } " << std::endl;
    //////////////////////////////////////////////////
      
    // write a indexedfaceset as the pixel grid, first all grid points
    if (it->second.name.length()>0)
      VRMLFile <<"   geometry DEF "<<CorrectDefString_(it->second.name)
               <<" IndexedFaceSet {"
               << std::endl;
    else
      VRMLFile <<"   geometry IndexedFaceSet {" << std::endl;
    VRMLFile <<"    coord Coordinate {" << std::endl;
    VRMLFile <<"     point [ " << std::endl;

    // get roi which is the original image
    // ImageData may be larger, because it is padded to power of two
    unsigned int x1,y1,x2,y2;
    it->second.ImageData.GetROI()->GetCorners(x1,y1,x2,y2);
    const unsigned int width  = x2;
    const unsigned int height = y2;
    for (unsigned int y=0; y<=height; y++) {
      for (unsigned int x=0; x<=width; x++) {

        const double xfrac = double(x) / double(width);
        const double yfrac = double(y) / double(height);

        //const double xfrac = (double(x)-0.5) / double(width-1);
       //const double yfrac = (double(y)-0.5) / double(height-1);
        Vector3<double> CurPoint(it->second.UL +
                                 xfrac*(it->second.UR-it->second.UL)+
                                 yfrac*( (1.0-xfrac) *
                                         (it->second.LL-it->second.UL)+
                                         xfrac*(it->second.LR-it->second.UR)));
        VRMLFile <<"     "
                 << CurPoint[0]-Center[0]<<" "<<CurPoint[1]-Center[1]
                 <<" "<<CurPoint[2]-Center[2] 
                 << ((y == height && x == width) ? " ] " : ", ")
                 << std::endl;
      }
    }
    VRMLFile <<"    } " << std::endl;
    //  now all the faces, the 3d pixels:
    VRMLFile <<"   coordIndex [ " << endl;
    for (unsigned int x=0; x<width;x++) {
      for (unsigned int y=0; y<height;y++) {
        VRMLFile <<"   "<< (x+y*(width+1)) << ", ";
        VRMLFile << (x+1+y*(width+1)) << ", ";
        VRMLFile << (x+1+(y+1)*(width+1)) <<", ";
        VRMLFile << (x+(y+1)*(width+1)) <<", -1"
                 << (((y+1) == height && (x+1) == width) ? " ] " : ", ")
                 << std::endl;
      }
    }

    // set color of the points
    VRMLFile << "    color Color {" << std::endl;
    VRMLFile << "     color [ " << std::endl;
    unsigned char** ppData = it->second.ImageData.GetImageDataArray();
    double r = 1.0;
    double g = 1.0;
    double b = 1.0;
    for (unsigned int x=0; x<width; x++) {
      for (unsigned int y=0; y<height; y++) {
        if (it->second.ImageData.GetChannelCount()==1) {
          r = g = b = ((double)ppData[y][x])/ 255.0;;
        } else if (it->second.ImageData.GetChannelCount()==3) {
          r = (double)ppData[y][3*x] / 255.0;
          g = (double)*(&(ppData[y][3*x])+1) / 255.0;
          b = (double)*(&(ppData[y][3*x])+2) / 255.0;
        } else {
          BIASERR("Not grey or rgb, cannot display !");
        }
        VRMLFile << "     "<<r<<" "<<g<<" "<<b
                 << (((y+1) == height && (x+1) == width) ? " ] " : ", ")
                 << std::endl;
      }
    }
    VRMLFile << "    } " <<std:: endl;   // terminates Color
    VRMLFile << "    colorPerVertex FALSE " <<std:: endl;
    VRMLFile << "   } " << std::endl;
    VRMLFile << "  } " << std::endl;
    if (it->second.BillBoard) {
      VRMLFile<<" ] "<<endl;
      VRMLFile<<"} " <<endl;

    }
    VRMLFile<<" ] "<<endl;
    VRMLFile << "} "<<endl;
  }
  return 0;
}


int ThreeDOut::VRMLOutLines(ostream& VRMLFile){
  if (Params_.LineWidth < 1) return 0;
  switch (Params_.LineStyle) {
  case Solid:
    for (map<unsigned int, Line3DData>::iterator it=mapLines_.begin();
         it!=mapLines_.end(); it++) {
      double r = (double)it->second.Color[0] / 255.0;
      double g = (double)it->second.Color[1] / 255.0;
      double b = (double)it->second.Color[2] / 255.0;
      // double a = (double)it->second.Color[3] / 255.0;
      VRMLFile << std::endl;
      VRMLFile << "Shape { "<<endl;
      VRMLFile << " appearance Appearance { " << endl;
      VRMLFile << "  material Material { " << endl;
      VRMLFile << "   transparency "<<VRMLtransparency(it->second.Color)<< endl;
      VRMLFile << "   emissiveColor " << r << " "<< g<< " "<<b << endl
               << "  } " << endl
               << " } "<<endl;

      VRMLFile <<" geometry IndexedLineSet { " << std::endl;
      VRMLFile <<"  coord Coordinate { " << std::endl;
      VRMLFile <<"   point [ " << std::endl;
      VRMLFile <<"   "<<it->second.Start[0]<<" "
               << it->second.Start[1]<<" " <<it->second.Start[2]<< " , "
               << std::endl;
      VRMLFile <<"   "<<it->second.End[0]<<" "
               <<it->second.End[1] <<" "<<it->second.End[2] <<" ] "<<std::endl;

      VRMLFile <<"  } " << std::endl;
      VRMLFile <<"  coordIndex [ 0, 1, -1 ] " << std::endl;
      VRMLFile <<"  color Color { " << std::endl;
      VRMLFile <<"   color [ "<<r<<" "<<g<<" "<<b<<" ] "<<std::endl;
      VRMLFile <<"  } " <<std:: endl;

      VRMLFile <<"  colorPerVertex FALSE " << std::endl;
      VRMLFile <<" } " << std::endl;
      VRMLFile <<"} " << std::endl;
    }
    break;
  default:
    BIASERR("Unsupported LineStyle, line drawing disabled.");
    return -1;
  }
  return 0;
}


void ThreeDOut::VRMLOutEllipsoid_(ostream& VRMLFile, Ellipsoid3DData& E,
                                  const BIAS::RGBAuc &Color)
{
  VRMLFile << "Transform { "<<endl;
  VRMLFile << " scaleOrientation "<< E.RotAxis[0] <<" "
           << E.RotAxis[1]<<" " <<E.RotAxis[2] <<" "<< E.dAngle <<" " << endl;
  VRMLFile << " scale "<< Params_.EllipsoidScale * E.s0 <<" "
           << Params_.EllipsoidScale * E.s1<< " "
           << Params_.EllipsoidScale * E.s2 << "" << endl;
  VRMLFile << " translation "
           << E.C[0] << " " << E.C[1] << " " << E.C[2] << " " << endl;
  VRMLFile << " children Shape { "<<endl;
  VRMLFile << "  appearance Appearance { " << endl;
  VRMLFile << "   material Material { " << endl;
  VRMLFile << "    transparency "<<VRMLtransparency(Color)<< endl;
  VRMLFile << "    diffuseColor " << (double)Color[0]/255.0 << " "
           << (double)Color[1]/255.0<< " "
           << (double)Color[2]/255.0 << endl;
  VRMLFile << "   }" << endl; // close Material
  VRMLFile << "  }" << endl; // close Appearance
  VRMLFile << "  geometry Sphere { radius 1.0 }" << endl;
  VRMLFile << " }" << endl; // close Shape
  VRMLFile << "}" << endl; // close Transform
}


int ThreeDOut::VRMLOutEllipsoids(ostream& VRMLFile, const BIAS::RGBAuc &Color)
{
  for (map<unsigned int, Ellipsoid3DData>::iterator it=mapEllipsoids_.begin();
       it!=mapEllipsoids_.end(); it++)
  {
    Ellipsoid3DData& E = it->second;
    VRMLOutEllipsoid_(VRMLFile, E, Color);
  }
  return 0;
}


int ThreeDOut::VRMLOutPointsAsSpheres_(std::ostream &VRMLFile)
{
  double thesize = Params_.PointSize;
  for (map<unsigned int, Point3DData>::iterator it=mapPoints_.begin();
    it!=mapPoints_.end(); it++) {
      Point3DData& E = it->second;
      VRMLFile << "Transform {"<<endl;
      VRMLFile << " translation "
               << E.Point[0] << " " << E.Point[1] << " " << E.Point[2] << endl;
      VRMLFile << " children Shape {"<<endl;
      VRMLFile << "  appearance Appearance { " << endl;
      VRMLFile << "   material Material { " << endl;
      VRMLFile << "    transparency "<<VRMLtransparency(E.Color)<<endl;
      VRMLFile << "    diffuseColor "
               << (double)E.Color[0]/255.0 << " "
               << (double)E.Color[1]/255.0 << " "
               << (double)E.Color[2]/255.0 << endl;
      VRMLFile << "    emissiveColor "
               << (double)E.Color[0]/255.0 << " "
               << (double)E.Color[1]/255.0 << " "
               << (double)E.Color[2]/255.0 << endl;
      VRMLFile << "   }" << endl; // close Material
      VRMLFile << "  }" << endl; // close Appearance
      VRMLFile << "  geometry Sphere { radius "<<thesize<<" }" << endl;
      VRMLFile << " }" << endl; // close Shape
      VRMLFile << "}" << endl; // close transform
    }
    return 0;
}


int ThreeDOut::VRMLOutEllipsoids(ostream& VRMLFile)
{
  for (map<unsigned int, Ellipsoid3DData>::iterator it=mapEllipsoids_.begin();
    it!=mapEllipsoids_.end(); it++) {
      Ellipsoid3DData& E = it->second;
      VRMLFile << "Transform {"<<endl;
      VRMLFile << " scaleOrientation "<< E.RotAxis[0] <<" "
               << E.RotAxis[1]<<" " <<E.RotAxis[2] <<" "<< E.dAngle <<" "
               << endl;
      VRMLFile << " scale "<< Params_.EllipsoidScale * E.s0 <<" "
               << Params_.EllipsoidScale * E.s1<< " "
               << Params_.EllipsoidScale * E.s2 << endl;
      VRMLFile << " translation "
               << E.C[0] << " " << E.C[1] << " " << E.C[2] << endl;
      VRMLFile << " children Shape { "<<endl;
      VRMLFile << "  appearance Appearance { " << endl;
      VRMLFile << "   material Material { " << endl;
      VRMLFile << "    transparency      "
               <<VRMLtransparency(E.Color)<<endl;
      VRMLFile << "    diffuseColor " << (double)E.Color[0]/255.0 << " "
               << (double)E.Color[1]/255.0<< " "
               << (double)E.Color[2]/255.0 << endl;
      VRMLFile << "   }" << endl; // close Material
      VRMLFile << "  }" << endl; // close Appearance
      VRMLFile << "  geometry Sphere { radius 1.0 }" << endl;
      VRMLFile << " }" << endl; // close Shape
      VRMLFile << "}" << endl; // close Transform
    }
    return 0;
}


int ThreeDOut::VRMLOut(const string &sFilename, const BIAS::RGBAuc &Color){
  ofstream VRMLFile;
  VRMLFile.open(sFilename.c_str());
  if (!VRMLFile) {
    BIASERR("Error opening VRML file for writing: "<<sFilename);
    return -1;
  }
  VRMLOutWriteHeader(VRMLFile);
  VRMLOutIndexedFaceSets(VRMLFile);
  VRMLOutEllipsoids(VRMLFile, Color);
  VRMLOutLines(VRMLFile, Color);
  VRMLOutPoints(VRMLFile, Color);
  VRMLOutImages(VRMLFile);
  VRMLOutPMatrices(VRMLFile, Color);
  if (Params_.WCSAxesLength>0.0) VRMLOutWCS(VRMLFile);
  VRMLFile.close();
  return 0 ;
}


int ThreeDOut::VRMLOut(const string &sFilename){
  ofstream VRMLFile;
  VRMLFile.open(sFilename.c_str());
  if (!VRMLFile) {
    BIASERR("Error opening VRML file for writing: "<<sFilename);
    return -1;
  }

  VRMLOutWriteHeader(VRMLFile);
  if (this->WriteViewpoints_)
    VRMLOutWriteViewpoint(VRMLFile);

  VRMLOutProjection(VRMLFile);

  VRMLOutIndexedFaceSets(VRMLFile);
  VRMLOutPoints(VRMLFile);
  VRMLOutEllipsoids(VRMLFile);
  VRMLOutLines(VRMLFile);
  VRMLOutImages(VRMLFile);
  VRMLOutPMatrices(VRMLFile);
  if (Params_.WCSAxesLength>0.0) VRMLOutWCS(VRMLFile);
  VRMLFile.close();
  return 0;
}


int ThreeDOut::VRMLOutWriteHeader(std::ostream& vrml){
  vrml << "#VRML V2.0 utf8" << endl;
  vrml << "# File written by Basic Image AlgorithmS (BIAS) library" << endl;
  vrml << "# http://www.mip.informatik.uni-kiel.de"
       <<endl;
  return 0;
}

// ------------------------------ other functions ----------------------------

void ThreeDOut::Dump(){
  cout <<endl<< "Dumping ThreeDOut object:"<<endl;
  cout <<"Points: -----------------------------------------"<<endl;
  for(map<unsigned int, Point3DData>::iterator it = mapPoints_.begin();
    it != mapPoints_.end(); it++) {
      cout <<"["<<it->first<<"] "<<it->second.Point<<endl;
    }
    cout<<endl<< "Lines: ------------------------------------------"<<endl;
    for(map<unsigned int, Line3DData>::iterator it = mapLines_.begin();
      it != mapLines_.end(); it++) {
        cout <<"["<<it->first<<"] "<<it->second.Start<<" -> "
          <<it->second.End<<endl;
      }
      cout<<endl<< "PMatrices: -------------------------------------- "<<endl;
      unsigned int i=0;
      for(map<unsigned int, PMatrix3DData>::iterator
        it = mapPMatrices_.begin(); it != mapPMatrices_.end(); it++) {
          cout <<"["<<(i++)<<"] UL="<<it->second.UL<<" UR="<<it->second.UR
            <<" LL="<<it->second.LL<<" LR="
            <<it->second.LR<<" C="<<it->second.C<<endl;
        }
        cout<<endl<< "Ellipsoids: -------------------------------------"<<endl;
        for(map<unsigned int, Ellipsoid3DData>::iterator it = mapEllipsoids_.begin();
          it != mapEllipsoids_.end(); it++) {
            cout <<"["<<it->first<<"] Center at "<<it->second.C<<
                   " size " <<it->second.s0<< " " << it->second.s1<< " " << it->second.s2 <<
                   " color " << it->second.Color <<endl;
            
          }
          cout<<endl<<flush;

}


Vector3<double> ThreeDOut::GetCenterOfPoints(){
  Vector3<double> center(0.0);
  unsigned int count=0;
  map<unsigned int, Point3DData>::iterator it;
  for (it=mapPoints_.begin(); it!=mapPoints_.end(); it++) {
    count ++;
    center += it->second.Point;
  }
  if(count > 0)
    center /= double(count);
  BIASWARN("The center of points cannot be determined, no points");
  return center;
}


void ThreeDOut::
GetBoundingBox(Vector3<double> &min, Vector3<double> &max)
{
  for (unsigned int i=0;i<3; i++) {
    min[i] = DBL_MAX;
    max[i] = -DBL_MAX;
  }

  map<unsigned int, Point3DData>::const_iterator it;
  for (it=mapPoints_.begin(); it!=mapPoints_.end(); it++) {
    for (unsigned int i=0;i<3; i++) {
      if (it->second.Point[i] < min[i]) min[i] = it->second.Point[i];
      if (it->second.Point[i] > max[i]) max[i] = it->second.Point[i];
    }
  }


  map<unsigned int, PMatrix3DData >::const_iterator pit;
  for (pit=mapPMatrices_.begin(); pit!=mapPMatrices_.end(); pit++){
    for (unsigned int i=0;i<3; i++) {
      if (pit->second.C[i] < min[i]) min[i] = pit->second.C[i];
      if (pit->second.C[i] > max[i]) max[i] = pit->second.C[i];
    }
  }

  map<unsigned int, IndexedFaceSet>::const_iterator fit;
  for (fit=mapIndexedFaceSet_.begin(); fit != mapIndexedFaceSet_.end(); fit++){
    const Vector3<double> &fmin(fit->second.bbmin);
    const Vector3<double> &fmax(fit->second.bbmax);
    for (unsigned int i=0;i<3; i++) {
      if (fmin[i] < min[i]) min[i] = fmin[i];
      if (fmax[i] > max[i]) max[i] = fmax[i];
    }
  }

  for (std::map<unsigned int, Image3DData>::iterator it= mapImages_.begin();
       it != mapImages_.end(); it++) {
    for (unsigned int i=0;i<3; i++) {
      if (it->second.UL[i]<min[i]) min[i] = it->second.UL[i];
      if (it->second.UR[i]<min[i]) min[i] = it->second.UR[i];
      if (it->second.LL[i]<min[i]) min[i] = it->second.LL[i];
      if (it->second.LR[i]<min[i]) min[i] = it->second.LR[i];
      if (it->second.UL[i]>max[i]) max[i] = it->second.UL[i];
      if (it->second.UR[i]>max[i]) max[i] = it->second.UR[i];
      if (it->second.LL[i]>max[i]) max[i] = it->second.LL[i];
      if (it->second.LR[i]>max[i]) max[i] = it->second.LR[i];
    }
  }

  for (std::map<unsigned int, Line3DData>::iterator it= mapLines_.begin();
       it !=mapLines_.end() ; it++) {
    for (unsigned int i=0;i<3; i++) {
      if (it->second.Start[i] < min[i]) min[i] = it->second.Start[i];
      if (it->second.Start[i] > max[i]) max[i] = it->second.Start[i];
      if (it->second.End[i] < min[i]) min[i] = it->second.End[i];
      if (it->second.End[i] > max[i]) max[i] = it->second.End[i];
    }
  }


  for (std::map<unsigned int, Ellipsoid3DData>::iterator pit=
         mapEllipsoids_.begin(); pit != mapEllipsoids_.end(); pit++) {
    double isoscale =  pit->second.s0;
    if (pit->second.s1>isoscale) isoscale=pit->second.s1;
    if (pit->second.s2>isoscale) isoscale=pit->second.s2;
    for (unsigned int i=0;i<3; i++) {
      if (pit->second.C[i]-isoscale < min[i]) min[i] =
                                                pit->second.C[i]-isoscale;
      if (pit->second.C[i]+isoscale > max[i]) max[i] =
                                                pit->second.C[i]+isoscale;
    }
  }

  for (std::map<unsigned int, Ellipsoid3DData>::iterator pit=
         mapCamEllipsoids_.begin(); pit != mapCamEllipsoids_.end(); pit++) {
    double isoscale =  pit->second.s0;
    if (pit->second.s1>isoscale) isoscale=pit->second.s1;
    if (pit->second.s2>isoscale) isoscale=pit->second.s2;
    for (unsigned int i=0;i<3; i++) {
      if (pit->second.C[i]-isoscale < min[i]) min[i] =
                                                pit->second.C[i]-isoscale;
      if (pit->second.C[i]+isoscale > max[i]) max[i] =
                                                pit->second.C[i]+isoscale;
    }
  }


  if (mapPMatrices_.empty() && mapPoints_.empty()
      && mapIndexedFaceSet_.empty() && mapLines_.empty() &&
      mapCamEllipsoids_.empty() && mapEllipsoids_.empty() &&
      mapImages_.empty()){
    //    BIASERR("empty bounding box in threedout object");
    for (unsigned int i=0;i<3; i++) {
      min[i] = 0;
      max[i] = -0;
    }
  }
}


void ThreeDOut::RemoveAll(){
  RemoveAllPoints();
  RemoveAllLines();
  RemoveAllEllipsoids();
  RemoveAllImages();
  RemoveAllPMatrices();
  RemoveAllIndexedFaceSets();
  RemoveAllCamEllipsoids();
}

// OpenGL interpretes alpha as opacity
// while VRML uses its inverse, transaparency
double ThreeDOut::VRMLtransparency(const unsigned char alpha){
  // our alpha stores opacity (as in GL)
  // but VRML uses transparency and normalized scaling
  return double( (255.0-double(alpha)) / 255.0 );
}

double ThreeDOut::VRMLtransparency(const BIAS::Vector4<unsigned char> & vRgba){
  return VRMLtransparency( vRgba[3] );
}


void ThreeDOut::SetWriteViewpoints(const bool & val){
  this->WriteViewpoints_ = val;
}


void ThreeDOut::VRMLOutWriteNavigationInfo(std::ostream& vrml)
{
  vrml<<"NavigationInfo {"<<endl;
  vrml<<"  avatarSize [0.1, 0.1, 0.1]"<<endl;
  vrml<<"  headlight TRUE"<<endl;
  vrml<<"  type [ \"EXAMINE\", \"ANY\" ]"<<endl;
  vrml<<"  visibilityLimit 0.0"<<endl;
  vrml<<"}"<<endl;
}


void ThreeDOut::VRMLOutWriteViewpoint(std::ostream& vrml)
{
  VRMLOutWriteNavigationInfo(vrml);

  map<unsigned int, PMatrix3DData>::iterator itP = mapPMatrices_.begin();
  //Vector3<double> up;
  int counter=0;
  //PMatrix3DData *it;

  for (itP=mapPMatrices_.begin(); (itP!=mapPMatrices_.end()) && (counter<=5); itP++){
    PMatrix3DData *p = &(itP->second);
    // compute fov:
    BIAS::Vector3<double> u,v;
    u = (p->UL +p->UR)*0.5 - p->C;
    v = p->PP           - p->C;
    BIASASSERT(u.Length()!=0);
    BIASASSERT(v.Length()!=0);
    double cosAlpha = u.ScalarProduct(v) / (u.Length() * v.Length());
    double alpha = acos(cosAlpha);

    // set virtual camera position, orientation and internal parameters:
    vrml<<"Viewpoint {"<<endl;
    vrml<<"  position "<<p->C[0]<<" "<<p->C[1]<<" "<<p->C[2]<<endl; // x y z pos.
    // default orientation is xright, y top, looking in -Z direction.
    // rotate 180 deg around x-axis to look to +Z:
    vrml<<"  orientation 1 0 0 "<<M_PI<<endl; // axis, angle rotate to +Z axis
    vrml<<"  fieldOfView "
      <<alpha/M_PI*180.0
      //<<45.0
      // TODO: P.GetFieldOfView(widthm height, true);
      <<endl;// fov
    vrml<<"  jump TRUE"<<endl; // immediate jump to position
    vrml<<"  description \"camera P"<<counter<<"\""<<endl;
    vrml<<"}"<<endl; // end of viewpoint

    counter++;
  }
}


void ThreeDOut::Translation(const Vector3<double>& movement) {

  for (std::map<unsigned int, PMatrix3DData >::iterator it =
         mapPMatrices_.begin();  it !=mapPMatrices_.end() ; it++) {
    it->second.C  += movement;
    it->second.UL += movement;
    it->second.UR += movement;
    it->second.LL += movement;
    it->second.LR += movement;
  }

  for (std::map<unsigned int, Image3DData>::iterator it= mapImages_.begin();
       it != mapImages_.end(); it++) {
    it->second.UL += movement;
    it->second.UR += movement;
    it->second.LL += movement;
    it->second.LR += movement;
  }


  for (std::map<unsigned int, Point3DData>::iterator it= mapPoints_.begin();
       it !=mapPoints_.end() ; it++) {
    it->second.Point += movement;
  }


  for (std::map<unsigned int, Line3DData>::iterator it= mapLines_.begin();
       it !=mapLines_.end() ; it++) {
    it->second.Start += movement;
    it->second.End += movement;
  }


  for (std::map<unsigned int, Ellipsoid3DData>::iterator it=
         mapEllipsoids_.begin(); it != mapEllipsoids_.end(); it++) {
    it->second.C += movement;
  }


  for (std::map<unsigned int, Ellipsoid3DData>::iterator it=
         mapCamEllipsoids_.begin(); it != mapCamEllipsoids_.end(); it++) {
    it->second.C += movement;
  }

  if (!mapIndexedFaceSet_.empty()) {
    for (std::map<unsigned int, IndexedFaceSet>::iterator it =
           mapIndexedFaceSet_.begin(); it != mapIndexedFaceSet_.end(); it++) {
      it->second.bbmin += movement;
      it->second.bbmax += movement;
#ifdef BIAS_HAVE_OPENGL
      GLfloat *VertArray = it->second.VertArray;
#endif
      for (std::vector<BIAS::Vector3<double> >::iterator itp =
             it->second.vertexSet.begin(); itp!=it->second.vertexSet.end();
           itp++){
        *itp += movement;
#ifdef BIAS_HAVE_OPENGL
        *VertArray++ += movement[0];
        *VertArray++ += movement[1];
        *VertArray++ += movement[2];
#endif
      }
#ifdef BIAS_HAVE_OPENGL
    // remove cached data structures
    if (it->second.compiled_ && it->second.displayList_ != 0) {
      glDeleteLists(it->second.displayList_, 1);
      glDeleteTextures(1, &it->second.textureObjectID_);
      it->second.compiled_ = false;
    }
#endif
    }

  }

  for (std::map<unsigned int, Projection3DData>::iterator it =
         mapProjection3D_.begin(); it != mapProjection3D_.end(); it++) {
    it->second.C += movement;
  }
}