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

/**
 * @file
 * @brief takes a texture list, a projection list and an output filename
 * and creates a vrml named with output filename with the cameras 
 * drawn, the textures as images in the camera pyramids, see biasVrmlOutCameraGeometry.cpp
 * @ingroup g_tools
 * @author MIP
 */


#include <Base/Image/ImageIO.hh>
#include <Base/ImageUtils/ImageDraw.hh>
#include <Base/Image/ImageConvert.hh>
#include <Image/Camera.hh>

#include <Utils/Param.hh>
#include <Utils/TriangleMesh.hh>
#include <GLviewer/ThreeDOutOpenSceneGraph.hh>

#include <iostream>



using namespace std;
using namespace BIAS;



void Usage()
{
  cout <<"Usage: biasVrmlOutCameraGeometry --projsList projs.lst --texList tex.lst --points3D points3D.txt --projectPoint true"<<endl
       <<endl;
  cout <<" The tool creates a vrml file with the projections drawn as mesh cameras. " << endl
       <<" If specified, it can add textures as images into the cameras, add 3D points to the scene " << endl
       <<" and project these 3D points into the images. If drawRays is set to true rays from cam center through 2D point" << endl
       <<" to 3D point will be drawn.  " << endl
       <<" ATTENTION: if projections are set in texture meta data projection list will be ignored!!! " << endl
       <<" In case the image should be just white instead of having a texture, set texList to WHITE." << endl
       <<" If the image should be black, set texList to BLACK."<< endl
       <<" If projsList is set to manualProjs.projection, a params file for projections can be given in the form: " << endl
       <<" width height " << endl
       <<" focalLength " << endl
       <<" centerX_1 centerY_1 centerZ_1 phiX_1 phiY_1 phiZ_1 " << endl
       <<" centerX_2 centerY_2 centerZ_2 phiX_2 phiY_2 phiZ_2 " << endl
       <<" ... " << endl
       <<" ATTENTION: in this case texList must either be set to WHITE or BLACK." << endl
       <<" ATTENTION: angles are specified in degrees." << endl
       <<" See bottom of tool implementation for example files with points and manual cams. " << endl
       <<" ATTENTION: only works with unsigned char textures for now." << endl
       <<endl<<endl;
}


void readPoints(string& pointsFile, vector<HomgPoint3D>& points3D){
  
  std::fstream filestr;
  filestr.open(pointsFile.c_str(), std::fstream::in);

  points3D.clear();

  double x, y, z;
  
  while (!filestr.eof()) {
    filestr >> x;
    filestr >> y;
    filestr >> z;
    if(x != -100.0 && y !=-100.0 && z != -100.0){
      cout << "read point " << x << " " << y << " " << z << endl;
      points3D.push_back(HomgPoint3D(x, y, z));
      x = -100.0;
      y = -100.0;
      z = -100.0;
    }
  }
 
  filestr.close();
}

void parseManualProjections(vector<Projection>& projs){
  
  std::fstream filestr;
  filestr.open(string("manualProjs.projection").c_str(), std::fstream::in);

  projs.clear();

  unsigned int width, height;
  double focalLength;
  
  double x, y, z;
  double phiX, phiY, phiZ;
  
  filestr >> width;
  filestr >> height;
  filestr >> focalLength;
  
  cout << "read width " << width << " height " << height << " focal length " << focalLength << endl;

  ProjectionParametersPerspective ppp;
  Projection projection;
  ppp.SetImageSize(width, height);
  ppp.SetFocalLengthAndAspect(focalLength, 1.0);
  ppp.SetPrincipal(width/2, height/2);
  ppp.SetSkew(0);
  
  projection.AddAbsoluteCamera(&ppp);
  
  Vector3<double> center;
  Quaternion<double> q;
  
  while (!filestr.eof()) {
    filestr >> x;
    filestr >> y;
    filestr >> z;
    filestr >> phiX;
    filestr >> phiY;
    filestr >> phiZ;
    if(x != -100.0 && y !=-100.0 && z != -100.0){
      cout << "read center " << x << " " << y << " " << z << endl;
      cout << "read rotation " << phiX << " " << phiY << " " << phiZ << endl;
      
      center[0] = x;
      center[1] = y;
      center[2] = z;

      q.SetXYZ(phiX/180.0*M_PI, phiY/180.0*M_PI, phiZ/180.0*M_PI);
      
      projection.SetQ(q);
      projection.SetC(center);
      
      cout << "finished projection " << projection << endl;
      projs.push_back(projection);
      
      x = -100.0;
      y = -100.0;
      z = -100.0;
      
    }
  }
 
  filestr.close();
  
  
}

// due to different interfaces, the colors for the points need to be created this way 
void createColors(vector<unsigned char>& reds, vector<unsigned char>& greens, vector<unsigned char>& blues){

  reds.clear();
  greens.clear();
  blues.clear();
    
  // red
  reds.push_back((unsigned char)200);
  greens.push_back((unsigned char)0);
  blues.push_back((unsigned char)0);

  // green
  reds.push_back((unsigned char)0);
  greens.push_back((unsigned char)200);
  blues.push_back((unsigned char)0);

  // blue
  reds.push_back((unsigned char)0);
  greens.push_back((unsigned char)0);
  blues.push_back((unsigned char)200);

  // yellow
  reds.push_back((unsigned char)200);
  greens.push_back((unsigned char)200);
  blues.push_back((unsigned char)0);

  // magenta
  reds.push_back((unsigned char)200);
  greens.push_back((unsigned char)0);
  blues.push_back((unsigned char)200);

  // cyan
  reds.push_back((unsigned char)0);
  greens.push_back((unsigned char)200);
  blues.push_back((unsigned char)200);
  
  // light grey
  reds.push_back((unsigned char)150);
  greens.push_back((unsigned char)150);
  blues.push_back((unsigned char)150);

  // dark grey
  reds.push_back((unsigned char)70);
  greens.push_back((unsigned char)70);
  blues.push_back((unsigned char)70);

  // purple 
  reds.push_back((unsigned char)130);
  greens.push_back((unsigned char)0);
  blues.push_back((unsigned char)170);

  
  // orange
  reds.push_back((unsigned char)255);
  greens.push_back((unsigned char)150);
  blues.push_back((unsigned char)0);
  
}

int main(int argc, char* argv[]){
  
  Param param;
  

 
  string *projsList = param.AddParamString("projsList", "list with projections", "");
  string *texList   = param.AddParamString("texList","list with projection files", "");
  string *points3DFile  = param.AddParamString("points3D","file with 3D points","");
  bool *projectPoints = param.AddParamBool("projectPoints", "set to true if 3D points are to be projected", false);
  string *outFile = param.AddParamString("outFile", "filename of resulting vrml file", "scene.wrl");
  bool *foreGroundLineColorBlack = param.AddParamBool("foreGroundLineColorBlack", "should foreground line colors be black or white", false);
  bool *drawRays = param.AddParamBool("drawRays", "should rays be drawn?", false);
  bool *draw3Dpoints = param.AddParamBool("draw3Dpoints","should 3D points be drawn or not", true);
  
  // make color vectors for points
  vector<unsigned char> reds;
  vector<unsigned char> greens;
  vector<unsigned char> blues;
  createColors(reds, greens, blues);
  
 
  // update parameters from file with arguments from command line
  if (argc == 1){
    Usage();
    return -1;
  }

  param.ParseCommandLine(argc, argv);

  param.ShowData(cout);
  
  // read lists if specified
  vector<string> projFiles;
  vector<Projection> projs;
  Projection P;
  if(*projsList != "" && *projsList != "manualProjs.projection"){
    Param::ParseListFile(*projsList, projFiles);
    for(unsigned int i=0; i < projFiles.size(); i++){
      P.Load(projFiles[i]);
      projs.push_back(P);
    }
  } else if(*projsList == "manualProjs.projection"){
    parseManualProjections(projs); 
  }
  
  // read texture filenames if specified
  vector<string> texFiles;
  if(*texList != "" && *texList != "WHITE" && *texList != "BLACK"){
    Param::ParseListFile(*texList, texFiles);
  }
  
  // read 3D points if specified
  vector<HomgPoint3D> points3D;
  if(*points3DFile != ""){
    readPoints(*points3DFile, points3D);
  }
  
  // init 3D out
  ThreeDOutParameters params3d;
  params3d.WCSAxesLength = -1;
  params3d.PointStyle = BIAS::Sphere;
  ThreeDOutOpenSceneGraph vrmlOut(params3d);

  // find out how many textures or projections there are
  unsigned int numImgs = projs.size();
  if(numImgs == 0){
    numImgs = texFiles.size();
  }

  // image holding texture
  Camera<unsigned char> texture;
  Camera<float> textureFl;
  
  // take care of each projection of the projection file list
  for(unsigned int i = 0; i < numImgs; i++){

    // state vars
    bool foundProjection = false;
    bool haveTex = false;
    
    // load texture
    bool searchFl = false;
    if(texFiles.size() == numImgs){ // texture files have been specified
      cout << "loading image... " << texFiles[i] << endl;
      if(ImageIO::Load(texFiles[i], texture)!=0) {
        cout<<"texture could not be loaded, trying to load float" << texFiles[i] << endl;
        if(ImageIO::Load(texFiles[i], textureFl)!=0){
          cout << "texture could still not be loaded, aborting" << endl;
          BIASABORT;
        } else {
          searchFl = true;
          textureFl.ScaleShift(255.0, 0);
          ImageConvert::ConvertST(textureFl, texture, ImageBase::ST_unsignedchar);
        }
      }
      
      haveTex = true;
      
      cout << "done ... parsing meta data..." << endl;
      texture.ParseMetaData();
      
      if(searchFl == false){
        if(texture.IsProjValid()){
          P = texture.GetProj();
          projs.push_back(P);
          foundProjection = true;
        }
        cout<<"finished reading texture \n";
      } else {
        textureFl.ParseMetaData();
        if(textureFl.IsProjValid()){
          P = textureFl.GetProj();
          projs.push_back(P);
          foundProjection = true;
        }
        cout<<"finished reading texture from fl \n";
        
      }
    } 

    
    // check if projection is present from projections list
    if(!foundProjection && projs.size() == numImgs) {
      foundProjection = true;
    } 

    // cast projection and add camera to vrml out
    ProjectionParametersBase* ppb = NULL;
    if(foundProjection == true){
      ppb = projs[i].GetParameters(0);
      unsigned int width, height;
      ppb->GetImageSize(width, height);
      ProjectionParametersPerspective* ppp = dynamic_cast<ProjectionParametersPerspective*>(ppb);
      if(ppp == NULL){
        cout << "only implemented for perspective case for now, returning" << endl;
        return -1;
      } 
      PMatrix pmatrix = ppp->GetP();
      if(*foreGroundLineColorBlack){
        vrmlOut.AddPMatrix(pmatrix, width, height, RGBAuc(0, 0, 0, 128), 1, "cam"+i);
      } else {
        vrmlOut.AddPMatrix(pmatrix, width, height, RGBAuc(255, 255, 255, 128), 1, "cam"+i);
      }
    }
    
    
    // make black or white image as texture if specified
    if(foundProjection && (*texList == "WHITE" || *texList == "BLACK")){
      unsigned int width=0, height=0;
      ppb->GetImageSize(width, height);
      texture.Init(width, height, 3, BIAS::ImageBase::ST_unsignedchar, true);
      unsigned char constColor[3];
      if(*texList == "BLACK"){
        constColor[0] = 0;
        constColor[1] = 0;
        constColor[2] = 0;
      } 
      if(*texList == "WHITE"){
        constColor[0] = 255;
        constColor[1] = 255;
        constColor[2] = 255;
      }
      texture.FillImageWithConstValue(constColor);
     // texFiles.push_back("const_" + i);
      haveTex = true;
    }

    
    // project 2D points
    if((*projectPoints) && haveTex && foundProjection){
      unsigned char* col = new unsigned char[3];
      for(unsigned int j=0; j < points3D.size(); j++){
        HomgPoint2D point2D;
        bool projected = projs[i].DoesPointProjectIntoImage(points3D[j], point2D);
        point2D.Homogenize();
        cout << "projected point " << point2D << " in image? " << projected << endl;
        if(projected){
          point2D.Homogenize();
          col[0] = reds[j%10];
          col[1] = greens[j%10];
          col[2] = blues[j%10];
          ImageDraw<unsigned char>::CircleCenterFilled(texture, (unsigned int)rint(point2D[0]), (unsigned int)rint(point2D[1]), (unsigned int)50, col);
          cout << "drawing point with color " << (int)rint(point2D[0]) << " " <<  (int)rint(point2D[1]) << " color " << (int)reds[j%10] << " "  <<  (int)greens[j%10] << " " << (int)blues[j%10] << endl;
          
          if(*drawRays){
            vrmlOut.AddLine(HomgPoint3D(projs[i].GetC()), points3D[j], RGBAuc(reds[j%10], greens[j%10], blues[j%10], 128));
          }
        }
        
      }
      cout << "might need to save texture now " << endl;
     
      delete col;
    }

    if(*texList=="WHITE" || *texList=="BLACK"){ // in case there no textures, they need to be created at this point
     stringstream texname;
     texname << "projImg_" << i << ".png";
     cout << "saving texture " << texname.str() << endl;
     texFiles.push_back(texname.str());
     ImageIO::Save(texFiles.back(), texture);
   }

    
    // create mes of texture
    if(haveTex){
      cout<<"meshing..."; cout.flush();
      TriangleMesh mesh(0.0,180.0);
      // make one quad in 3d space
      mesh.GenerateImagePlane(projs[i], texture);


      vrmlOut.AddTriangleMesh(mesh, "mesh_from_"+string(texFiles[i]),
                            texFiles[i], true);

    }
    
  } // end of for loop projections/ images
  
    
  if(*draw3Dpoints){
    for(unsigned int i=0; i < points3D.size(); i++){
      cout << "adding point with color " << reds[i%10] << " " << greens[i%10] << " " << blues[i%10] << endl;
      vrmlOut.AddPoint(points3D[i], RGBAuc(reds[i%10], greens[i%10], blues[i%10], 128));
    }
  }
  
  cout<<"Writing osg to " << *outFile;

  vrmlOut.OpenSceneGraphOut(*outFile);
  
  cout<<" finished\n";

  return 0;
}

/* Example file for manual cameras
 * It depicts for cameras facing each other in a way that it works for making slides
 * 
 * 1200  900
 * 1000
 * 0    0     1   30  140   0
 * 4    0     1   40  220   0
 * 3    0    -4  -20 -20    0
 * 0    0    -4  -20  20    0
 * 
*/

/* 3D point list with points that are seen in the cam list above
 * 2    0.5 -0.5
 * 1.5  1.2 -1.2
 * 1    0.9 -1.6
 * 1.3  1.3 -1.4
 * 0.8  1.5 -2
 * 1.5  1   -2.1
 *  
 */