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

/**
 * @example ExampleRectification.cpp
   @relates RectificationBase, PlanarRectification, CylindricalRectification, SphericalRectification
   @brief Example for image rectification.
   @ingroup g_examples
   @author MIP
*/

#include <iostream>
#include <Base/Image/ImageIO.hh>
#include <Image/Camera.hh>
#include <Image/PlanarRectification.hh>
#include <Image/CylindricalRectification.hh>
#include <Image/SphericalRectification.hh>
#include <Image/RectificationBase.hh>
#include <Utils/Param.hh>
#include <Utils/ThreeDOut.hh>
#include <Base/Image/ImageConvert.hh>

using namespace std;
using namespace BIAS;

//#define OutStorageType float
#define OutStorageType unsigned char


int main(int argc, char* argv[])
{
  Param params(true);
  bool* vrml = params.AddParamBool("writeVrml",
                                   "Write out VRML with rectified geometry?",
                                   false);

  bool* pointsonly =
    params.AddParamBool("pointsOnly",
                        "Do not use texture but colored vertices instead?",
                        false);

  bool* help = params.AddParamBool("help", "", false, 'h');

  enum {cylindrical = 0, spherical, planar}; // rectTypeID;

  vector<string> rectificationTypes;
  rectificationTypes.push_back("cylindrical");//0
  rectificationTypes.push_back("spherical");  //1
  rectificationTypes.push_back("planar");     //2
  int* rectType =
    params.AddParamEnum("type",
                        "Specifies type of rectification to perform:"
                        " [cylindrical], spherical, planar",
                        rectificationTypes);

  enum {Trilinear = 0, Bilinear};
  vector<string> interpolationType;
  interpolationType.push_back("Trilinear"); // = 0
  interpolationType.push_back("Bilinear");  // = 1
  int* interType =
    params.AddParamEnum("interpolation",
                        "Specifies type interpolation to use:"
                        " [Trilinear], Bilinear",
                        interpolationType);

  double* resolution =
    params.AddParamDouble("resolution",
                          "Scales the resolution of rectified images",
                          0.8, 0.001);

  bool* useLUT =
    params.AddParamBool("useLUT",
                        "Turn off usage of lookup tables", true);

  string* rectSetupName =
    params.AddParamString("rectSetup", "File that contains or shall contain the"
                          " rectification setup", "", 0, -1);

  int fup = params.ParseCommandLine(argc, argv);

  if(argc<3 || *help) {
    cout<<"Determines rectification and stores them in name."<<endl<<endl;
    cout<<"Usage: "<<argv[0]<<" [options] <origImgA> <origImgB>"<<endl;
    params.Usage();
    return 0;
  }

  BIAS::Camera<unsigned char> RGBcamA, camA, RGBcamB, camB;

  if(ImageIO::Load(argv[fup], camA)!=0) {
    cerr<<"[ERROR] Unable to load first camera!"<<endl;
    return -1;
  }

  if(ImageIO::Load(argv[fup+1], camB)!=0) {
    cerr<<"[ERROR] Unable to second first camera!"<<endl;
    return -1;
  }

  BIAS::Camera<float> rectSetup;
  bool useSetup = false;
  bool generateSetup = false;
  if(rectSetupName->size()>0) {
    useSetup = true;
    if(ImageIO::Load(*rectSetupName, rectSetup)<0) {
      cerr<<"[ERROR] Could not open rectification setup file "
          <<*rectSetupName<<endl;
      cerr<<"        It will be created from given parameters."<<endl;
      generateSetup = true;
    }
  }
  //camA.ParseMetaData();
  //camB.ParseMetaData();

  BIAS::RectificationBase<unsigned char, OutStorageType>* rectifier = NULL;
  BIAS::RectificationViaProjectionMappingBase<unsigned char, OutStorageType>*
    rectifierPM = NULL;

  // Choose different rectification methods here
  switch(*rectType) {
    case cylindrical :
      rectifier = rectifierPM =
        new CylindricalRectification<unsigned char, OutStorageType>(1.3);
      break;
    case spherical :
      rectifier = rectifierPM =
        new SphericalRectification<unsigned char, OutStorageType>();
      break;
    case planar :
      rectifier = rectifierPM =
        new PlanarRectification<unsigned char, OutStorageType>();
      break;
    default:
      cerr<<"[ERROR] Unknown rectification type "
          <<*rectType<<" given!"<<endl;
      return -1;
  }

  // Choose different interpolation methods here
  switch(*interType) {
    case Trilinear:
      rectifierPM->SetInterpolationMethod(MapTrilinear);
      break;
    case Bilinear:
      rectifierPM->SetInterpolationMethod(MapBilinear);
      break;
    default:
      cerr<<"[ERROR] Unknown interpolation method "
          <<*interType<<" given!"<<endl;
      return -1;
  }
  rectifierPM->UseLookUpTables(*useLUT);
  rectifierPM->SetScale(*resolution);
  rectifierPM->SetSecondFill(255);
  if( rectifier->SetCameraA(camA) != 0 ) {
    cerr<<"[ERROR] Invalid camera type for camera A!"<<endl;
    delete rectifier;
    return -1;
  }

  if( rectifier->SetCameraB(camB) != 0 ) {
    cerr<<"[ERROR] Invalid camera type for camera B!"<<endl;
    delete rectifier;
    return -1;
  }

  if(useSetup) {
    if(generateSetup) {
      if(rectifierPM->GetRectificationSetup(rectSetup)<0) {
        cerr<<"[ERROR] Failed to create rectification setup!"<<endl;
        return -1;
      }
      //if(ImageIO::Save(*rectSetupName, rectSetup)<0) {
      if(ImageIO::Save(*rectSetupName, rectSetup)<0) {
        cerr<<"[ERROR] Failed to write rectification setup to "
            <<*rectSetupName<<endl;
        return -1;
      }
    } else {
      if(rectifierPM->SetRectificationSetup(rectSetup)<0) {
        cerr<<"[ERROR] Failed to load rectification setup!"<<endl;
        return -1;
      }
    }
  }

  ProjectionParametersBase* rectPPA;
  ProjectionParametersBase* rectPPB;

  if(rectifierPM->GetClonesOfRectificationParameters(rectPPA, rectPPB) != 0) {
    cerr<<"[ERROR] Failed to retrieve rectification parameters!"<<endl;
    delete rectifier;
    return -1;
  }

  cerr<<"Found rectification parameters!"<<endl;
  cerr<<"- Camera A rectification state: "<<*rectPPA<<endl;
  cerr<<"- Camera B rectification state: "<<*rectPPB<<endl<<endl;

  cerr<<"Starting rectification... "<<flush;
  if(rectifier->Rectify()!=0) {
    cerr<<" failed!"<<endl;
    delete rectifier;
    delete rectPPA;
    delete rectPPB;
    return -1;
  }
  cerr<<" done!"<<endl;

  Camera<OutStorageType> rectA, rectB, RGBrectA, RGBrectB;

  rectifier->GetRectifiedImageA(rectA);
  rectA.SetProj(Projection(*rectPPA));
  rectA.UpdateMetaData();

  rectifier->GetRectifiedImageB(rectB);
  rectB.SetProj(Projection(*rectPPB));
  rectB.UpdateMetaData();

  string path1, basename1, suffix1;
  string path2, basename2, suffix2;
  FileHandling::SplitName(argv[fup], path1, basename1, suffix1);
  FileHandling::SplitName(argv[fup+1], path2, basename2, suffix2);

  string firstName = basename1+"_rect";//+suffix1;
  string secondName = basename2+"_rect";//+suffix2;


  if(ImageIO::Save(firstName, rectA, ImageIO::FF_mip)!=0) {
    cerr<<"[ERROR] Failed to write first rectified image!"<<endl;
  }
  if(ImageIO::Save(secondName, rectB, ImageIO::FF_mip)!=0) {
    cerr<<"[ERROR] Failed to write second rectified image!"<<endl;
  }

  if(*vrml) {
    ThreeDOut tdo;
    tdo.AddProjection(camA.GetProj(), RGBAuc(0, 255, 0, 255));
    tdo.AddProjection(camB.GetProj(), RGBAuc(0, 255, 0, 255));
//  tdo.AddProjection(Projection(*rectPPA), RGBAuc(255, 0, 0, 255));
//  tdo.AddProjection(Projection(*rectPPB), RGBAuc(255, 0, 0, 255));

    cerr<<"Generating VRML models with rectification data:"<<endl;
    cerr<<"- First VRML containing coordinate systems only... "<<flush;
    tdo.AddLine(rectPPA->GetC(), rectPPB->GetC());
    bool good = false;
    if(tdo.VRMLOut("rectCoordinates.wrl")!=0) {
      good = false;
    } else {
      good = true;
    }
    if(good) {
      cerr<<"done!";
    } else {
      cerr<<"failed!";
    }
    cerr<<endl;

    cerr<<"- Generating VRML models containing projected images... "<<flush;
    unsigned int width = 0, height = 0;
    rectPPA->GetImageSize(width, height);
    Image<float> depthmap(width, height, 1);
    depthmap.SetZero();

    double scale = 3.0;
    if (*pointsonly) {
      for (unsigned int y=0; y<height; y++) {
        for (unsigned int x=0; x<width; x++) {
          HomgPoint2D p2(x,y);
          HomgPoint3D p3 = rectPPA->UnProjectToImagePlane(p2, scale*0.99);
          if (p3.NormL2()<0.1) continue;
          OutStorageType color = rectA.GetImageDataArray()[y][x];
          RGBAuc theColor((unsigned char)color,(unsigned char)color,
                          (unsigned char)color,255);
          tdo.AddPoint(p3, theColor);
        }
      }
    } else {
      RGBrectA.Init(rectA.GetWidth(), rectA.GetHeight(), 3);
      ImageConvert::ToRGB(rectA, RGBrectA);
      TriangleMesh T;
      Camera<unsigned char> tmp;
      ImageConvert::ConvertST(RGBrectA, tmp, ImageBase::ST_unsignedchar);
      T.GenerateTexturedCamera(rectPPA, tmp, scale*0.99) ;
      tdo.AddTriangleMesh(T, "textureA.png", "textureA.png");
    }

    if (*pointsonly) {
      rectPPB->GetImageSize(width, height);
      for (unsigned int y=0; y<height; y++) {
        for (unsigned int x=0; x<width; x++) {
          HomgPoint2D p2(x,y);
          HomgPoint3D p3 = rectPPB->UnProjectToImagePlane(p2, scale*1.01);
          if (p3.NormL2()<0.1) continue;
          unsigned char color =
            (unsigned char)rint(rectB.GetImageDataArray()[y][x]);
          RGBAuc theColor(color,color,color,255);
          tdo.AddPoint(p3, theColor);
        }
      }
    } else {
      RGBrectB.Init(rectB.GetWidth(), rectB.GetHeight(), 3);
      ImageConvert::ToRGB(rectB, RGBrectB);
      TriangleMesh T;
      Camera<unsigned char> tmp;
      ImageConvert::ConvertST(RGBrectB, tmp, ImageBase::ST_unsignedchar);
      T.GenerateTexturedCamera(rectPPB, tmp, scale*1.01) ;
      tdo.AddTriangleMesh(T, "textureB.png", "textureB.png");
    }
    Vector3<double> c= rectPPA->GetC() - rectPPB->GetC();
    cerr<<"done!"<<endl<<flush;

    tdo.VRMLOut("rectCoordinates_image.wrl");
    cerr<<"- Writing mipview3d animation file 'cylindersweep.anim'... "<<flush;
    ofstream myanimfile("cylindersweep.anim");
    myanimfile <<"[0] :43 0 0 0 0 1 0 0 0"<<endl
               <<"[0] :43 1000 " <<c[0]<<" "<<c[1]<<" "<<c[2]
               <<" 1 0 0 0"<<endl;
    cerr<<"done!"<<endl;
    cerr<<"- Generating VRML models with original cameras... "<<flush;

    ThreeDOut tdo2;
    RGBcamA.Init(camA.GetWidth(), camA.GetHeight(), 3);
    ImageConvert::ToRGB(camA, RGBcamA);
    RGBcamB.Init(camB.GetWidth(), camB.GetHeight(), 3);
    ImageConvert::ToRGB(camB, RGBcamB);

    TriangleMesh T1, T2;
    T1.GenerateTexturedCamera(camA.GetProj().GetParameters(), RGBcamA, scale);
    T2.GenerateTexturedCamera(camB.GetProj().GetParameters(), RGBcamB, scale);
    tdo2.AddTriangleMesh(T1, "textureAorig.png", "textureAorig.png");
    tdo2.AddTriangleMesh(T2, "textureBorig.png", "textureBorig.png");
    tdo2.AddProjection(camA.GetProj(), RGBAuc(0, 255, 0, 255),
                       scale*0.1, scale);
    tdo2.AddProjection(camB.GetProj(), RGBAuc(0, 255, 0, 255),
                       scale*0.1, scale);
    tdo2.VRMLOut("orig.wrl");
    cerr<<"done!"<<endl<<flush;
  }

  delete rectPPA;
  delete rectPPB;
  delete rectifier;

  return 0;
}