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

#include <Base/Common/BIASpragma.hh>

#include <Image/ProjectionMapping.hh>
#include <Base/Image/ImageIO.hh>
#include <Base/Image/ImageConvert.hh>
#include <Base/Image/Image.hh>
#include <Base/Debug/TimeMeasure.hh>
#include <Base/Math/Vector3.hh>
#include <Base/Math/Vector.hh>
#include <Utils/Param.hh>
#include <Utils/ThreeDOut.hh>
#include <Geometry/ProjectionParametersSpherical.hh>
#include <Geometry/ProjectionParametersZoom.hh>
#include <Image/Camera.hh>
#include <Filter/DeInterlace.hh>
#include <Utils/IOUtils.hh>

using namespace BIAS;
using namespace std;

/** @file
    @ingroup g_tools
    @brief rotate/rectify/reproject images to (rotated)
    images (of different camera type), see biasproject.cpp
    @author oniemann / koeser */

/** \internal */
enum MappingType
      {UNDEF, PERSPECTIVE, PERSPECTIVE_ZOOM, SPHERICAL};

void IdealizeAspectRatio(KMatrix& K, unsigned int& width, unsigned int& height){

  double factor = K[0][0] / K[1][1];
  if (factor<1.0) {
    K[1][1] = K[0][0];
    K[1][2] = factor * K[1][2];
    height = (unsigned int)(rint(double(height)*factor));
  } else {
    K[0][0] = K[1][1];
    K[0][2] = K[1][2] / factor;
    width = (unsigned int)(rint(double(width) / factor));
  }
}

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

  MappingType mappingType = UNDEF;
  Param *param = new Param(true);
  string sourceImageName,sinkImageName,sourceCamName,sinkCamName;

  // this is ugly but we want to support float and unsigned char
  // if more datatypes are needed, then we might think of computing always in
  // float and then going back to the type we want
  ProjectionMapping<unsigned char, unsigned char> mapper_uc;
  ProjectionMapping<float, float> mapper_float;
  Projection sourceCam,sinkCam;
  unsigned int width, height;
  ImageBase testimg;
  bool ComputeInFloat = false;
  Camera<unsigned char> sourceimguc, sinkimguc;
  Camera<float> sourceimgfloat, sinkimgfloat;

  InterpolationMethod interpolation_Method = MapTrilinear;

  param->AddParamString("SourceImage",
                        "The source Image",
                        "",
                        'i');

  param->AddParamString("SourceImageList",
                        "A List of source Images, where each image has the same properties "
                        "(e.g. from same camera). If set, this will be used instead of SourceImage and output filenames will be bp-OrigName",
                        "",
                        'l');

  param->AddParamString("SinkImage",
                        "Where to save the sink Image",
                        "",
                        'o');

  param->AddParamString("SourceCam",
                       "The source Camera calibration XML",
                       "",
                       'c');

  param->AddParamString("SinkCam",
                       "The sink Camera calibration XML filename or leave "
                        "empty, if you want an ideal image (no distortion)",
                        "",
                        's');

  param->AddParamInt("Interpolation",
                     "Which interpolation shall be used?",
                     3,
                     0,
                     4,
                     'I');

  param->AddParamBool("deinterlace",
                      "use first and then every other image row",
                      false, 'd');

  string* lut = param->AddParamString("LUT",
                                      "file containing LUT, if not existent "
                                      "appropriate LUT with name is created.",
                                      "");

  bool* leaveAspect = param->AddParamBool("leaveAspect",
                                      "don't change aspect ratio during undistortion", false);

  double* rescale = param->AddParamDouble("rescale",
    "rescale factor applied to sink camera parameters",
    1.0, 0.0, DBL_MAX, 'r');

  //param->ParseCommandLine(argc,argv);
  if(!IOUtils::ParseCommandLineEvalHelp(*param, argc, argv)) {
    cout<<endl<<"Usage example for image undistortion (if projection in header):"
      <<endl<<endl<<"\t biasproject -i distorted.pgm"<<endl
      <<endl<<" for unidealized aspct ratio use :"
      <<endl<<endl<<"\t biasproject -i distorted.pgm --leaveAspect"<<endl;
    return 0;
  }

  if(argc < 2){
    param->Usage();
    BIASERR(endl<<
            "Usage example for image (with projection in header) undistortion:"
            <<endl<<endl
            <<" biasproject -i distorted.pgm"<<endl);
    return -1;
  }

  //Read and load source image and projection from parameters
  sourceImageName = *param->GetParamString("SourceImage");
  sourceCamName = *param->GetParamString("SourceCam");
  sinkImageName = *param->GetParamString("SinkImage");
  string sourceImageListName = *param->GetParamString("SourceImageList");

  vector<string> sourceImageList;
  vector<string> sinkImageList;
  //bool useList = false;
  if (sourceImageListName.size() > 0) {
    // in case a list of images is given, parse it and create corresponding output names
    if(Param::ParseListFile(sourceImageListName, sourceImageList)!=0) {
      BIASERR("error parsing source image names!");
      return -1;
    } else {
      //useList = true;
      sourceImageName = sourceImageList[0];
      sinkImageList.resize(sourceImageList.size());
      for (unsigned int i=0; i < sourceImageList.size(); i++) {
        string dir;
        string base;
        string suffix;
        FileHandling::SplitName(sourceImageList[i], dir, base, suffix );
        sinkImageList[i] = string("bp-")+ base + suffix;
      }
    }
  } else {
    // no list given, use sourceImageName as only image to process
    sourceImageList.push_back(sourceImageName);
    if (sinkImageName.size()==0) {
      string dir;
      string base;
      string suffix;

      FileHandling::SplitName(sourceImageName, dir, base, suffix );
      sinkImageName = string("bp-")+ base + suffix;
      cout<<"using output image name "<<sinkImageName<<" for input file "<<
        sourceImageName<<endl;
    }
    sinkImageList.push_back(sinkImageName);
  }

  sinkCamName = *param->GetParamString("SinkCam");
  switch(*param->GetParamInt("Interpolation"))
  {
    case 0: interpolation_Method = MapNearestNeighbor;break;
    case 1: interpolation_Method = MapBilinear;break;
    case 2: interpolation_Method = MapBicubic;break;
    case 3: interpolation_Method = MapTrilinear;break;
    case 4: interpolation_Method = MapAnisotropic;break;
    default:
      BIASERR("unknown interpolation_Method");
  }

  bool deinterlace =  *param->GetParamBool("deinterlace");

  // check if there is already a LUT file
  bool UseLUT = false;
  bool generateLUT = false;
  bool mapperPrepared = false;
  if(lut->size()!=0) {
    UseLUT = true;
    fstream file;
    file.open(lut->c_str());
    if(!file.good()) {
      cerr<<"could not load lut, will generate lut file!\n";
      generateLUT = true;
    }
    file.close();
  }

  // foreach in source image list
  for (unsigned int i=0;i<sourceImageList.size(); i++) {

    cout << "BIASPROJECT: " << sourceImageList[i] << " -> " << sinkImageList[i] << endl;

    if (ImageIO::Load(sourceImageList[i], testimg)!=0){
      BIASERR("error loading image "<<sourceImageList[i]);
      return -2;
    }
    ImageBase* sourceimg = NULL;
    if (testimg.GetStorageType()==ImageBase::ST_float) {
      sourceimgfloat = Camera<float>(testimg);
      if (deinterlace) {
        cout<<"deinterlacing .."<<endl<<flush;
        DeInterlace<float, float> D;
        D.SetDeInterlaceType(BIAS_DI_121);
        D.Filter(Camera<float>(testimg), sourceimgfloat);
      }
      sourceimg = &sourceimgfloat;
      sourceimguc.SetMetaData(*sourceimgfloat.GetMetaData());
      ComputeInFloat = true;
    } else if (testimg.GetStorageType()==ImageBase::ST_unsignedchar) {
      sourceimguc = Camera<unsigned char>(testimg);
      if (deinterlace) {
        cout<<"deinterlacing .."<<endl<<flush;
        DeInterlace<unsigned char, unsigned char> D;
        D.SetDeInterlaceType(BIAS_DI_121);
        D.Filter(Camera<unsigned char>(testimg), sourceimguc);
      }
      sourceimg = &sourceimguc;
      sourceimgfloat.SetMetaData(*sourceimguc.GetMetaData());
      ComputeInFloat = false;

    } else {
      BIASERR("unsupported data type in input image (float and uchar supported)");
      exit(-1);
    }
    sourceimgfloat.ParseMetaData();
    sourceimguc.ParseMetaData();

    if ((sourceimg->GetColorModel()!=ImageBase::CM_Grey) &&
        (sourceimg->GetColorModel()!=ImageBase::CM_RGB) && (sourceimg->GetColorModel()!=ImageBase::CM_Depth)) {
      cout<<"For safety your color format is converted to RGB (e.g. Bayer),"
          <<" so channelwise interpolation causes no problems."<<endl;
      if (ComputeInFloat)
        ImageConvert::ToRGB(sourceimgfloat, sourceimgfloat);
      else
        ImageConvert::ToRGB(sourceimguc, sourceimguc);
    }

    if (sourceCamName=="" && sourceimguc.IsProjValid()) {
      sourceCam = sourceimguc.GetProj();
      sourceCam.XMLWrite("source.projection");
    } else if (sourceCam.Load(sourceCamName)!=0) {
      BIASERR("Error reading projection (leave empty if in meta data)"
              <<sourceCamName);
      return -3;
    }
    BIASASSERT(sourceCam.GetParameters()!=NULL);


    if (sinkCamName.size()==0) {
      // in this mode we simply use the same output as input projection
      // but we set aspect ratio=1, skew = 0 and distortion=0.
      // works therefore only for perspective cameras
      sinkCam = sourceCam;

      ProjectionParametersPerspective *PPP = NULL;
      ProjectionParametersZoom *PPZ = NULL;
      ProjectionParametersSpherical *PPS = NULL;


        PPP = dynamic_cast<ProjectionParametersPerspective *>(sinkCam.GetParameters());
      if (PPP!=NULL) {
        mappingType = PERSPECTIVE;
        PPP->SetUndistortion(0,0,0,0);
      }else{
        PPZ = dynamic_cast<ProjectionParametersZoom *>(sinkCam.GetParameters());

        if(PPZ!=NULL){
          mappingType = PERSPECTIVE_ZOOM;
        }else{
          PPS = dynamic_cast<ProjectionParametersSpherical *>(sinkCam.GetParameters());

          if(PPS!=NULL){
            mappingType = SPHERICAL;
          }else{
            cerr << "Sorry, but your projection parameters don't match any case that can be computed." << endl
                 << "Please use tool biasprojectspherical for sphere undistortion" << endl;
            return -5;
          }
        }
      }


      KMatrix K;
      switch(mappingType){
        case PERSPECTIVE:
          K = PPP->GetK();
          K[0][1] = 0;
          if(!(*leaveAspect)){
            unsigned int width=0, height=0;
            PPP->GetImageSize(width, height);
            IdealizeAspectRatio(K,width,height);
            PPP->SetImageSize(width,height);
          }
          PPP->SetK(K);
          break;

        case PERSPECTIVE_ZOOM:
          // not sure if one can set an alternative aspect ratio to a zoom image
          break;
        case SPHERICAL:
          cerr << "Not implemented yet" << endl;
          return -1;
          break;
        default:
          break;
      }


    } else if (sinkCam.Load(sinkCamName)!=0) {
      BIASERR("Error reading "<<sinkCamName);
      return -3;
    }
    BIASASSERT(sinkCam.GetParameters()!=NULL);

    if(*rescale > 0.0 && *rescale != 1.0) {
      sinkCam.GetParameters()->Rescale(*rescale);
    }

    sinkCam.GetParameters()->GetImageSize(width,height);
    cout<<"ROI of source image is "<<*sourceimg->GetROI()
        <<" while image size is "<<width<<" x "<<height<<endl;



    if (interpolation_Method<MapTrilinear) {
      cout<<"You disabled anti-aliasing, mapping with "<<
        interpolation_Method<<endl;
    }

    if (ComputeInFloat){
      if (sinkimgfloat.IsEmpty())
        sinkimgfloat.Init(width,height,sourceimgfloat.GetChannelCount());
      if (!mapperPrepared) {
        mapper_float.SetSinkCam(sinkCam);
        mapper_float.SetSourceCam(sourceCam);
        if(generateLUT) {
          mapper_float.PrepareLookupTableMapping(sourceimgfloat, sinkimgfloat, interpolation_Method);
          mapper_float.GetLookupTable(*lut, interpolation_Method);
        } else {
          if(UseLUT) {
            if (mapper_float.SetLookupTable(*lut, interpolation_Method) != 0) {
              BIASERR("Setting lut from file failed");
            } else {
              cerr << "successfully loaded lut from file" << endl;
            }
          }
        }
        mapperPrepared = true;
      }
      if(UseLUT) {
        mapper_float.MapWithLookupTable(sourceimgfloat, sinkimgfloat, interpolation_Method);
      } else {
        mapper_float.Map(sourceimgfloat, sinkimgfloat, interpolation_Method);
      }
      sinkimgfloat.SetMetaData(*sourceimgfloat.GetMetaData());
      sinkimgfloat.SetProj(sinkCam);
      sinkimgfloat.UpdateMetaData();

      if (ImageIO::Save(sinkImageList[i], sinkimgfloat)!=0){
        BIASERR("error writing image to disk");
      }
    } else {
      if (sinkimguc.IsEmpty())
        sinkimguc.Init(width,height,sourceimguc.GetChannelCount());
      if (!mapperPrepared) {
        mapper_uc.SetSinkCam(sinkCam);
        mapper_uc.SetSourceCam(sourceCam);
        if(generateLUT) {
          mapper_uc.PrepareLookupTableMapping(sourceimguc, sinkimguc, interpolation_Method);
          mapper_uc.GetLookupTable(*lut, interpolation_Method);
        } else {
          if(UseLUT) {
            if (mapper_uc.SetLookupTable(*lut, interpolation_Method) != 0) {
              BIASERR("Setting lut from file failed");
            } else {
              cerr << "successfully loaded lut from file" << endl;
            }
          }
        }
        mapperPrepared = true;
      }
      if(UseLUT) {
        mapper_uc.MapWithLookupTable(sourceimguc, sinkimguc, interpolation_Method);
      } else {
        mapper_uc.Map(sourceimguc, sinkimguc, interpolation_Method);
      }
      sinkimguc.SetMetaData(*sourceimguc.GetMetaData());
      sinkimguc.SetProj(sinkCam);
      sinkimguc.UpdateMetaData();
      if (ImageIO::Save(sinkImageList[i], sinkimguc, ImageIO::FF_auto,
                        BIAS_DEFAULT_SYNC, BIAS_DEFAULT_IMAGE_QUALITY,
                        true, true)!=0){
        BIASERR("error writing image to disk");
      }
    }
  }
  delete param;
  return 0;
}