biasrectify.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 <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

/**
   @file
   @ingroup g_tools
   @brief Tool for rectifying images, see  biasrectify.cpp
   @author bartczak
*/

int findLargestOverlappingRegion(const ProjectionParametersBase *camAproj,
                                 const ProjectionParametersBase *camBproj,
                                 const ProjectionParametersBase *rectAproj,
                                 const ProjectionParametersBase *rectBproj, ROI &roi);

int
findLargestOverlappingRegion(ProjectionParametersBase* camAproj,
                             ProjectionParametersBase* camBproj,
                             ProjectionParametersPerspective* rectAproj,
                             ProjectionParametersPerspective* rectBproj,
                             const double* zeroParallaxZ, ROI& roiA, ROI& roiB,
                             bool verbose = false);

int findLargestRectangularRegion(const ProjectionParametersBase *camProj,
                                 const ProjectionParametersBase *rectProj, ROI& roi);

double CalculateRescaleFactor(int targetWidth, int targetHeight, int roiWidth,
                              int roiHeight);

void CentralCropRoi(int targetWidth, int targetHeight, ROI& roi);


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

  //GROUP Misc.
  int groupID = params.GetFreeGroupID();
  bool* help = params.AddParamBool("help", "", false, 'h', groupID);
  params.SetGroupName(groupID, "Misc.");

  //GROUP 3D visualisation
  groupID = params.GetFreeGroupID();
  bool* vrml = params.AddParamBool("writeVrml",
                                   "write out vrml with rect geometry", false, 0, groupID);
  bool* pointsonly = params.AddParamBool("pointsonly",
                                         "dont use texture,but colored vertices", false, 0, groupID);
  params.SetGroupName(groupID, "3D Visualisation");

  //GROUP Main Settings
  groupID = params.GetFreeGroupID();
  enum {
    cylindrical = 0, spherical, planar, planarKeepAspect
  }; // rectTypeID;
  vector<string> rectificationTypes;
  rectificationTypes.push_back("cylindrical");//0
  rectificationTypes.push_back("spherical"); //1
  rectificationTypes.push_back("planar"); //2
  rectificationTypes.push_back("planarKeepAspect"); //3
  int* rectType = params.AddParamEnum("type",
                                      "what kind of rectification shall be performed?:"
                                      " [cylindrical], spherical, planar, planarKeepAspect",
                                      rectificationTypes, 0, NULL, 0, groupID);

  enum {
    Trilinear = 0, Bilinear
  };
  vector<string> interpolationType;
  interpolationType.push_back("Trilinear");//0
  interpolationType.push_back("Bilinear");//1
  int* interType = params.AddParamEnum("interpolation",
                                       "what kind of interpolation shall be performed?:"
                                       " [Trilinear], Bilinear", interpolationType, 0, NULL, 0,
                                       groupID);

  double* rescale = params.AddParamDouble("rescale",
                                          "scales the resolution of rectified images", 0.8, 0.001, DBL_MAX,
                                          0, groupID);

  bool* useLUT = params.AddParamBool("useLUT",
                                     "turn off the use of look up tables", false, 0, groupID);

  params.SetGroupName(groupID, "Main Settings");

  //GROUP Cropping
  groupID = params.GetFreeGroupID();

  //   int* destHeight =
  //     params.AddParamInt("destHeight",
  //                           "height of overlapping view from both cameras, if set, resolution is calculated.",
  //                        -1, -1, 10000, 0, groupID);

  //   int* destWidth =
  //     params.AddParamInt("destWidth",
  //                           "width of overlapping view from both cameras, if set, resolution is calculated.",
  //                           -1, -1, 10000, 0, groupID);

  Vector<int>
    * targetResolution =
    params.AddParamVecInt(
      "targetResolution",
      "Give the resolution of the fully filled rectangle in the rectified images, "
      "that shall be possible to crop."
      " If both components are negative, this feature is disabled, using the rescale parameter."
      " If one component is above 0 the parameter rescale is not used.",
      "-1 -1", 0, groupID);

  bool* cropImages = params.AddParamBool("crop",
                                         "if target resolution is given, crop resulting images to these values. "
                                         "Delivers the cropped results as output.", false, 0, groupID);

  double
    * zeroParallaxZ =
    params.AddParamDouble(
      "zeroParallaxZ",
      "give the zero parallax z-distance in rectified space. "
      "From this the cropping and rescale parameters can be adapted. "
      "A value below zero indicates infinity! Only works with planar rectification.",
      -1, -1, DBL_MAX, 0, groupID);

  string
    * cropInformationFile =
    params.AddParamString(
      "cropInfo",
      "writes the information to be used for cropping after rectification into this text file.",
      "", 0, groupID);

  bool
    * writeOutCropParams =
    params.AddParamBool(
      "writeCroppedCamParams",
      "If this is active no cropping is performed. "
      "Only the camera parameters fitting the cropped, rectified images are written out.",
      false, 0, groupID);

  params.SetGroupName(groupID, "Cropping");

  //GROUP Cropping
  groupID = params.GetFreeGroupID();

  string* leftList = params.AddParamString("leftList", "leftImagesList", "",
                                           0, groupID);
  string* rightList = params.AddParamString("rightList", "leftImagesList",
                                            "", 0, groupID);

  string* leftCamParams = params.AddParamString("leftCamParams",
                                                "left camera Params", "", 0, groupID);

  string* rightCamParams = params.AddParamString("rightCamParams",
                                                 "left camera Params", "", 0, groupID);

  string* outdir = params.AddParamString("outputDir", "output directory",
                                         ".", 0, groupID);

  bool* keepType = params.AddParamBool("keepType", "keep filetype of image",
                                       false, 0, groupID);

  string* newType = params.AddParamString("newType", "new image type", "png",
                                          0, groupID);

  bool* writeRectParams = params.AddParamBool("writeRectParams",
                                              "write out rectified camera parameters", false, 0, groupID);

  int* bordercolor2 = params.AddParamInt("borderSecond",
                                         "color of border in second view", 255, 0, 255, 0, groupID);

  string* rectSetupName = params.AddParamString("rectSetup",
                                                "file that contains or shall contain the rect setup", "", 0,
                                                groupID);

  bool* forceModulo4size = params.AddParamBool("forceModulo4size",
                                               "Result images are extended "
                                               "at the lower and/or right edges to fit into modulo 4 size."
                                               "!) Camera parameters are adapted accordingly."
                                               "!) Feature is disabled if crop is true.", false, 0, groupID);

  params.SetGroupName(groupID, "I/O");

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

  if (*help) {
    cout << "\nusage:\n";
    cout << argv[0] << " [options] <origImgA> <origImgB>\n";
    params.Usage();
    return 0;
  }

  if (*cropImages && *writeOutCropParams) {
    cout << "crop and writeCroppedCamParams are both active!\n";
    cout << "writeCroppedCamParams is ambiguouse when crop is active!\n";
    cout << "Turn one option off!\n";
    return -1;
  }

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

  //   if(leftList->empty() && rightList->empty() ) {

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

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

  Camera<float> rectSetup;
  bool useSetup = false;
  bool generateSetup = false;
  if (rectSetupName->size() > 0) {
    useSetup = true;
    if (ImageIO::Load(*rectSetupName, rectSetup) < 0) {
      cerr << "could not open rectified setup file " << *rectSetupName
           << endl;
      cerr << "will create it from given params .\n";
      generateSetup = true;
    }
  }
  //   camA.ParseMetaData();
  //   camB.ParseMetaData();

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

  //choose different rectification at this point
  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;
    case planarKeepAspect: {
      cout << "Using planar rectification with preserved aspectratio" << endl;
      PlanarRectification<unsigned char, OutStorageType>* rectifierPlanar;
      rectifier = rectifierPM = rectifierPlanar = new PlanarRectification<
      unsigned char, OutStorageType> ();
      rectifierPlanar->RetainOriginalApectRatio();
    }
      break;
    default:
      cerr << " unknown rectification type : "
           << rectificationTypes[*rectType] << endl;
      return -1;
      break;
  }

  switch (*interType) {
    case Trilinear:
      rectifierPM->SetInterpolationMethod(MapTrilinear);
      break;
    case Bilinear:
      rectifierPM->SetInterpolationMethod(MapBilinear);
      break;
  }

  int destWidth = (*targetResolution)[0];
  int destHeight = (*targetResolution)[1];

  if (destHeight > 0 || destWidth > 0) {
    *rescale = 1.0f;
  } else {
    if (*cropImages) {
      cout
        << "cropping requested but no destination image dimensions set. disabling crop"
        << endl;
      *cropImages = false;
    }
  }

  rectifierPM->UseLookUpTables(*useLUT);
  rectifierPM->SetScale(*rescale);
  rectifierPM->SetSecondFill((unsigned char)(*bordercolor2));

  unsigned int imageNum;
  vector<string> leftImageNames;
  vector<string> rightImageNames;
  if (leftList->empty() && rightList->empty()) {
    imageNum = 1;
    if (fup + 1 >= argc) {
      cout << "You must specify image list files with --leftList and --rightList " << endl
           << "or two image file names as parameters!" << endl;
      return 0;
    }
    leftImageNames.push_back(argv[fup]);
    rightImageNames.push_back(argv[fup + 1]);
  } else {
    if (Param::ParseListFile(*leftList, leftImageNames) != 0) {
      BIASERR("error parsing left list");
      return -1;
    }
    if (Param::ParseListFile(*rightList, rightImageNames) != 0) {
      BIASERR("error parsing right list");
      return -1;
    }
    imageNum = leftImageNames.size();
    BIASASSERT(imageNum == rightImageNames.size());
  }

  Projection camAproj, camBproj;
  if (!leftCamParams->empty()) {
    if (camAproj.Load(*leftCamParams) != 0) {
      BIASERR("error reading left camera parameters");
    }
  }
  if (!rightCamParams->empty()) {
    if (camBproj.Load(*rightCamParams) != 0) {
      BIASERR("error reading right camera parameters");
    }
  }

  ProjectionParametersBase* rectPPA = NULL;
  ProjectionParametersBase* rectPPB = NULL;

  // roi for max overlapping view
  ROI roi;

  for (unsigned int i = 0; i < imageNum; i++) {

    //load image data
    if (ImageIO::Load(leftImageNames[i], camA) != 0) {
      cerr << "unable to load first camera '" << leftImageNames[i] << "'"
          << endl;
      return -1;
    }

    if (ImageIO::Load(rightImageNames[i], camB) != 0) {
      cerr << "unable to load second camera '" << rightImageNames[i] << "'"
          << endl;
      return -1;
    }

    //set the parameters ?
    if (leftCamParams->empty() && i == 0) {
      camA.ParseMetaData();
      if (!camA.IsProjValid()) {
        BIASERR("right camera projection not specified in image!");
        return -1;
      }
      camAproj = camA.GetProj();

    }
    if (rightCamParams->empty() && i == 0) {
      camB.ParseMetaData();
      if (!camB.IsProjValid()) {
        BIASERR("right camera projection not specified in image!");
        return -1;
      }
      camBproj = camB.GetProj();
    }

    if (rectifier->SetCameraA(camA, camAproj.GetParameters()) < 0) {
      cerr << "invalid camera type for camera A!\n";
      delete rectifier;
      return -1;
    }

    if (rectifier->SetCameraB(camB, camBproj.GetParameters()) < 0) {
      cerr << "invalid camera type for camera B!\n";
      delete rectifier;
      return -1;
    }

    if (useSetup && i == 0) {
      if (generateSetup) {
        if (rectifierPM->GetRectificationSetup(rectSetup) < 0) {
          cerr << "rectification setup could not be created!\n";
          return -1;
        }
        //if (ImageIO::Write(*rectSetupName, rectSetup) < 0) {
        if (ImageIO::Save(*rectSetupName, rectSetup) < 0) {
          cerr << "error writing rectification setup into "
               << *rectSetupName << endl;
          return -1;
        }
      } else {
        if (rectifierPM->SetRectificationSetup(rectSetup) < 0) {
          cerr << "rectification setup could not be loaded!\n";
          return -1;
        }
      }
    }

    if (i == 0) {

      if (rectifierPM->GetClonesOfRectificationParameters(rectPPA,
                                                          rectPPB) != 0) {
        cerr << "rectification parameters could not be retrieved!\n";
        delete rectifier;
        return -1;
      }

      /*  cout<<"Initial rectification parameters : \n";
          cout<<"CamA :\n";
          cout<<*rectPPA<<endl;
          cout<<"CamB :\n";
          cout<<*rectPPB<<endl;*/

      if ((destHeight > 0 || destWidth > 0)) {
        if ((*rectType != planar && *rectType != planarKeepAspect)) {
          BIASERR(
            "cropping is only valid for planarily rectified images!");
          return -1;
        }
        // falko's reference code:
        //
        //       findLargestOverlappingRegion(camAproj.GetParameters(), camBproj.GetParameters(), rectPPA, rectPPB, roi);
        //       double scaleH = 0;
        //       double scaleW = 0;
        //       int roiheight = roi.GetCornerLowerRightY() - roi.GetCornerUpperLeftY();
        //       int roiwidth = roi.GetCornerLowerRightX() - roi.GetCornerUpperLeftX();
        //       if (destHeight > 0)
        //         scaleH = float(destHeight) / roiheight;
        //       if (destWidth > 0 )
        //         scaleW = float(destWidth) / roiwidth;
        //       double scale = (scaleW > scaleH) ? scaleW : scaleH;
        //       rectifierPM->SetScale(scale);
        //       rectifierPM->GetClonesOfRectificationParameters(rectPPA, rectPPB);
        //       findLargestOverlappingRegion(camAproj.GetParameters(), camBproj.GetParameters(), rectPPA, rectPPB, roi);
        //       roiheight = roi.GetCornerLowerRightY() - roi.GetCornerUpperLeftY();
        //       roiwidth = roi.GetCornerLowerRightX() - roi.GetCornerUpperLeftX();
        //       // adapt parameters to cut roi
        //       if (*cropImages) {
        //         double x,y;
        //         rectPPA->GetPrincipal(x,y);
        //         int xoffset = 0, yoffset = 0;
        //         if (roiwidth > destWidth) {
        //           xoffset = (roiwidth - destWidth) / 2;
        //         }
        //         if (roiheight > destHeight) {
        //           yoffset = (roiheight - destHeight) / 2;
        //         }
        //         rectPPA->SetPrincipal(x+roi.GetCornerUpperLeftX()+xoffset,y+roi.GetCornerUpperLeftY()+yoffset);
        //         rectPPA->SetImageSize(roiwidth-2*xoffset,roiheight-2*yoffset);
        //         rectPPB->GetPrincipal(x,y);
        //         rectPPB->SetPrincipal(x+roi.GetCornerUpperLeftX()+xoffset,y+roi.GetCornerUpperLeftY()+yoffset);
        //         rectPPB->SetImageSize(roiwidth-2*xoffset,roiheight-2*yoffset);
        //         roi.SetCorners(
        //                        roi.GetCornerUpperLeftX()+xoffset,
        //                        roi.GetCornerUpperLeftY()+yoffset,
        //                        roi.GetCornerLowerRightX()-xoffset,
        //                        roi.GetCornerLowerRightY()-yoffset
        //                        );
        //       }

        //try to get it going with zero parallax
        ROI roiA, roiB;
        //      cout<<"find inital rect area:\n";
        findLargestOverlappingRegion(camAproj.GetParameters(),
                                     camBproj.GetParameters(),
                                     (ProjectionParametersPerspective*) rectPPA,
                                     (ProjectionParametersPerspective*) rectPPB,
                                     zeroParallaxZ, roiA, roiB);
        //roiA and roiB have same width and same height
        int roiHeight = roiA.GetCornerLowerRightY()
          - roiA.GetCornerUpperLeftY();
        int roiWidth = roiA.GetCornerLowerRightX()
          - roiA.GetCornerUpperLeftX();

        double adaptedScale = CalculateRescaleFactor(destWidth,
                                                     destHeight, roiWidth, roiHeight);
        cout << "rescale for cropping : " << adaptedScale << endl;
        rectifierPM->SetScale(adaptedScale);

        rectifierPM->GetClonesOfRectificationParameters(rectPPA,
                                                        rectPPB);
        //     cout<<"find rect area after scale:\n";
        findLargestOverlappingRegion(camAproj.GetParameters(),
                                     camBproj.GetParameters(),
                                     (ProjectionParametersPerspective*) rectPPA,
                                     (ProjectionParametersPerspective*) rectPPB,
                                     zeroParallaxZ, roiA, roiB, true);
        //unsigned int adaptedWidth, adaptedHeight;
        //         rectPPB->GetImageSize(adaptedWidth, adaptedHeight);
        //         roiA.Resize(adaptedWidth, adaptedHeight);
        //         roiB.Resize(adaptedWidth, adaptedHeight);
        //         //adapt rois
        //         roiA.SetCorners(rint(adaptedScale*roiA.GetCornerUpperLeftX()),
        //                         rint(adaptedScale*roiA.GetCornerUpperLeftY()),
        //                         rint(adaptedScale*roiA.GetCornerLowerRightX()),
        //                         rint(adaptedScale*roiA.GetCornerLowerRightY()));

        //         roiB.SetCorners(rint(adaptedScale*roiB.GetCornerUpperLeftX()),
        //                         rint(adaptedScale*roiB.GetCornerUpperLeftY()),
        //                         rint(adaptedScale*roiB.GetCornerLowerRightX()),
        //                         rint(adaptedScale*roiB.GetCornerLowerRightY()));

        /*    cout<<"roiA before central crop: "<<roiA<<endl;
              cout<<"roiB before central crop: "<<roiB<<endl;*/

        //now determine the where to crop
        CentralCropRoi(destWidth, destHeight, roiA);
        CentralCropRoi(destWidth, destHeight, roiB);

        /*cout<<"roiA after central crop: "<<roiA<<endl;
          cout<<"roiB after central crop: "<<roiB<<endl;*/

        if (cropInformationFile->size() > 0) {
          string outfileName = *outdir + "/" + *cropInformationFile;
          //now write out the crop parameters
          ofstream ofs(outfileName.c_str());
          ofs << "**               BIASrectify             **\n"
              << "** crop information for rectified images **\n\n";

          ofs << "---> zero parallax = " << *zeroParallaxZ
              << " <---\n\n";

          ofs << "---> left images <---\n"
              << "crop rect upper left ["
              << roiA.GetCornerUpperLeftX() << ", "
              << roiA.GetCornerUpperLeftY() << "] (included)\n"
              << "crop rect lower right ["
              << roiA.GetCornerLowerRightX() << ", "
              << roiA.GetCornerLowerRightY() << "] (excluded)\n";
          ofs << "\n\n";
          ofs << "---> right images <---\n"
              << "crop rect upper left ["
              << roiB.GetCornerUpperLeftX() << ", "
              << roiB.GetCornerUpperLeftY() << "] (included)\n"
              << "crop rect lower right ["
              << roiB.GetCornerLowerRightX() << ", "
              << roiB.GetCornerLowerRightY() << "] (excluded)\n";

          ofs
            << "\n\nATTENTION: Don't forget to adapt disaprity values if you are cropping disparity maps! \n";
          ofs
            << "The disparity values have to be changed by an amount of "
            << abs(
              static_cast<int> (roiB.GetCornerUpperLeftX())
              - static_cast<int> (roiA.GetCornerUpperLeftX()))
            << " disparities.\n";
          ofs
            << "Whether to decrease or increase disparity values, depends on the definition of disparity and on the view of the cropped disparity map!\n";
          ofs.close();
        }

        if (*cropImages || *writeOutCropParams) {
          ProjectionParametersBase* rectPPATmp;
          ProjectionParametersBase* rectPPBTmp;

          rectifierPM->GetClonesOfRectificationParameters(rectPPATmp,
                                                          rectPPBTmp);
          double x, y;
          rectPPATmp->GetPrincipal(x, y);
          rectPPATmp->SetPrincipal(x - roiA.GetCornerUpperLeftX(), y
                                   - roiA.GetCornerUpperLeftY());
          rectPPATmp->SetImageSize(destWidth, destHeight);
          rectPPBTmp->GetPrincipal(x, y);
          rectPPBTmp->SetPrincipal(x - roiB.GetCornerUpperLeftX(), y
                                   - roiB.GetCornerUpperLeftY());
          rectPPBTmp->SetImageSize(destWidth, destHeight);

          if (*writeOutCropParams) {
#ifdef BIAS_HAVE_XML2
            Projection(*rectPPATmp).XMLWrite(*outdir+"/camLeftRectified_cropped.xml");
            Projection(*rectPPBTmp).XMLWrite(*outdir+"/camRightRectified_cropped.xml");
            //cout<<"written out cropped parameters terminating. \n";
#else
            BIASWARN(
              "requested writing of camera parameters, but xml2 not enabled in BIAS");
#endif
            //return 0;
            delete rectPPATmp;
            delete rectPPBTmp;
          } else {

            PlanarRectification<unsigned char, OutStorageType>
              * rectifierPlanar =
              dynamic_cast<PlanarRectification<
            unsigned char, OutStorageType>*> (rectifierPM);
            rectifierPlanar->FixateIntrinsics(
              *(ProjectionParametersPerspective*) rectPPATmp,
              *(ProjectionParametersPerspective*) rectPPBTmp);
            rectifierPlanar->SetScale(1.0);
            delete rectPPA;
            delete rectPPB;
            rectPPA = rectPPATmp;
            rectPPB = rectPPBTmp;
          }
        } // crop or write out cropped params end

      }

      cerr << "Found rectification parameters! \n ";
      if (*forceModulo4size && !*cropImages) {
        unsigned int width, height;
        rectPPA->GetImageSize(width, height);
        int missingW = width % 4;
        int missingH = height % 4;
        if (missingW != 0) {
          width = width + (4 - missingW);
        }
        if (missingH != 0) {
          height = height + (4 - missingH);
        }
        rectPPA->SetImageSize(width, height);
        rectPPB->GetImageSize(width, height);
        missingW = width % 4;
        missingH = height % 4;
        if (missingW != 0) {
          width = width + (4 - missingW);
        }
        if (missingH != 0) {
          height = height + (4 - missingH);
        }
        rectPPB->SetImageSize(width, height);
      }
      cerr << "camera A rectification state:\n" << *rectPPA << endl
           << endl;
      cerr << "camera B rectification state:\n" << *rectPPB << endl
           << endl;
      cerr << "rectifying ... " << endl;

    }//if i == 0
    //    cerr<<"."<<flush;


    if (rectifier->Rectify() != 0) {
      cerr << " failed \n";
      delete rectifier;
      delete rectPPA;
      delete rectPPB;
      return -1;
    }

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

    rectifier->GetRectifiedImageA(rectA);
    //     // cut to roi
    //     if (*cropImages) {
    //       rectA.SetROI(roi);
    //       rectA.Cut2ROI();
    //     } //is now integrated into the
    if (*forceModulo4size != 0 && !*cropImages) {
      unsigned int width, height;
      rectPPA->GetImageSize(width, height);
      /*int missingW = width%4;
        int missingH = height%4;*/
      //rectPPA->SetImageSize(width+missingW, height+missingH);
      rectA.Pad(width, height, 0);
    }
    rectA.SetProj(Projection(*rectPPA));
    rectA.UpdateMetaData();

    rectifier->GetRectifiedImageB(rectB);
    //     // cut to roi
    //     if (*cropImages) {
    //       rectB.SetROI(roi);
    //       rectB.Cut2ROI();
    //     }
    if (*forceModulo4size != 0 && !*cropImages) {
      unsigned int width, height;
      rectPPB->GetImageSize(width, height);
      /* int missingW = width%4;
         int missingH = height%4;*/
      // rectPPB->SetImageSize(width+missingW, height+missingH);
      rectB.Pad(width, height, *bordercolor2);
    }
    rectB.SetProj(Projection(*rectPPB));
    rectB.UpdateMetaData();

    if (*writeRectParams) {
#ifdef BIAS_HAVE_XML2
      Projection(*rectPPA).XMLWrite(*outdir+"/camLeftRectified.xml");
      Projection(*rectPPB).XMLWrite(*outdir+"/camRightRectified.xml");
#else
      BIASWARN(
        "requested writing of camera parameters, but xml2 not enabled in BIAS");
#endif
    }

    string path1, basename1, suffix1;
    string path2, basename2, suffix2;
    FileHandling::SplitName(leftImageNames[i], path1, basename1, suffix1);
    FileHandling::SplitName(rightImageNames[i], path2, basename2, suffix2);

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

    if(*cropImages) {
      firstName += "_cropped";
      secondName += "_cropped";
    }
    
    if (*keepType) {
      if (ImageIO::Save(firstName + suffix1, rectA) != 0) {
        cerr << "writing first rectified image failed!\n";
      }
      if (ImageIO::Save(secondName + suffix2, rectB) != 0) {
        cerr << "writing first rectified image failed!\n";
      }
    } else {
      if (newType->compare("") != 0) {
        if (ImageIO::Save(firstName + "." + *newType, rectA) != 0) {
          cerr << "writing first rectified image failed!\n";
        }
        if (ImageIO::Save(secondName + "." + *newType, rectB) != 0) {
          cerr << "writing first rectified image failed!\n";
        }
      } else {
        if (ImageIO::Save(firstName, rectA, ImageIO::FF_mip) != 0) {
          cerr << "writing first rectified image failed!\n";
        }
        if (ImageIO::Save(secondName, rectB, ImageIO::FF_mip) != 0) {
          cerr << "writing first rectified image failed!\n";
        }
      }
    }

    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 vrmls with rectification data : \n";
      cerr << "1. vrml containing only coordinate systems ... " << flush;
      tdo.AddLine(rectPPA->GetC(), rectPPB->GetC());
      bool good = false;
      if (tdo.VRMLOut("rectCoordinates.wrl") != 0) {
        good = false;
      } else {
        good = true;
      }
      if (good) {
        cerr << "finished";
      } else {
        cerr << "failure";
      }
      cerr << endl;

      cerr << "2. generating vrml 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 << " writing ..." << endl << flush;
      tdo.VRMLOut("rectCoordinates_image.wrl");
      cerr << "writing mipview3d-animfile cylindersweep.anim" << endl;
      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 << "3. generating wrl with orig cameras" << endl << 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");
      cerr << " writing ..." << endl << flush;
      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");
    }
  } //for each image pair

  delete rectPPA;
  delete rectPPB;
  delete rectifier;

  cout << " done!\n";
  return 0;
}

int findLargestRectangularRegion(ProjectionParametersBase* origProj,
                                 ProjectionParametersBase* rectProj, ROI& roi) {

  PixelIterator it;
  unsigned int w, h;
  rectProj->GetImageSize(w, h);
  roi.Resize(w, h);
  //  Image<unsigned char> testIm(w,h, 1);
  //  unsigned char **ida = testIm.GetImageDataArray();
  unsigned int xmin = 0, xmax = w, ymin = 0, ymax = h;
  // mid point
  unsigned int mx, my;
  origProj->GetImageSize(mx, my);
  HomgPoint2D midPoint(mx / 2, my / 2, 1);
  Vector3<double> midRay, pos;
  origProj->UnProjectToRay(midPoint, pos, midRay);
  HomgPoint3D ray3D;
  ray3D[0] = midRay[0];
  ray3D[1] = midRay[1];
  ray3D[2] = midRay[2];
  ray3D[3] = 0;
  midPoint = rectProj->Project(ray3D);
  origProj->GetFirstBorderPixel(it);
  HomgPoint2D edgepoint(0, 0, 0);
  do {
    HomgPoint2D sourcepoint(it.x, it.y, 1);
    HomgPoint2D oldPoint = edgepoint;
    Vector3<double> globalray;
    origProj->UnProjectToRay(sourcepoint, pos, globalray);
    ray3D[0] = globalray[0];
    ray3D[1] = globalray[1];
    ray3D[2] = globalray[2];
    ray3D[3] = 0;
    edgepoint = rectProj->Project(ray3D);
    // get distance vector to midpoint
    HomgPoint2D dist = edgepoint - midPoint;
    HomgPoint2D grad = edgepoint - oldPoint;

    // going right (or left)
    if (abs(grad[0]) > abs(grad[1])) {
      if (dist[1] < 0) {
        if (edgepoint[1] > ymin) {
          ymin = (unsigned)rint(edgepoint[1]);
        }
      } else {
        if (edgepoint[1] < ymax) {
          ymax = (unsigned)rint(edgepoint[1]);
        }
      }
    } // going up (or down)
    else {
      if (dist[0] < 0) {
        if (edgepoint[0] > xmin) {
          xmin = (unsigned)rint(edgepoint[0]);
        }
      } else {
        if (edgepoint[0] < xmax) {
          xmax = (unsigned)rint(edgepoint[0]);
        }
      }
    }

    // discard points not in image
    //      if (edgepoint[0] >= 0 && edgepoint[0] < w && edgepoint[1] >= 0 && edgepoint[1] < h) {
    //        ida[ (int)edgepoint[1] ][ (int)edgepoint[0] ] = 100;
    //      }
  } while (origProj->GetNextBorderPixel(it));

  roi.SetCorners(xmin, ymin, xmax, ymax);
  //cout<<"roi "<<roi<<endl;
  return 0;

}

int findLargestOverlappingRegion(const ProjectionParametersBase *camAproj,
                                 const ProjectionParametersBase *camBproj,
                                 const ProjectionParametersBase *rectAproj,
                                 const ProjectionParametersBase *rectBproj, ROI &roi) {

  vector<ProjectionParametersBase *> source;
  vector<ProjectionParametersBase *> dest;
  source.resize(2);
  dest.resize(2);
  source[0] = (ProjectionParametersBase*) camAproj;
  source[1] = (ProjectionParametersBase*) camBproj;
  dest[0] = (ProjectionParametersBase*) rectAproj;
  dest[1] = (ProjectionParametersBase*) rectBproj;
  PixelIterator it;
  unsigned int w, h;
  rectAproj->GetImageSize(w, h);
  //  Image<unsigned char> testIm(w,h, 1);
  //  unsigned char **ida = testIm.GetImageDataArray();
  unsigned int xmin = 0, xmax = w, ymin = 0, ymax = h;
  for (unsigned int i = 0; i < source.size(); i++) {
    // mid point
    unsigned int mx, my;
    source[i]->GetImageSize(mx, my);
    HomgPoint2D midPoint(mx / 2, my / 2, 1);
    Vector3<double> midRay, pos;
    source[i]->UnProjectToRay(midPoint, pos, midRay);
    HomgPoint3D ray3D;
    ray3D[0] = midRay[0];
    ray3D[1] = midRay[1];
    ray3D[2] = midRay[2];
    ray3D[3] = 0;
    midPoint = dest[i]->Project(ray3D);
    source[i]->GetFirstBorderPixel(it);
    HomgPoint2D edgepoint(0, 0, 0);
    do {
      HomgPoint2D sourcepoint(it.x, it.y, 1);
      HomgPoint2D oldPoint = edgepoint;
      Vector3<double> globalray;
      source[i]->UnProjectToRay(sourcepoint, pos, globalray);
      ray3D[0] = globalray[0];
      ray3D[1] = globalray[1];
      ray3D[2] = globalray[2];
      ray3D[3] = 0;
      edgepoint = dest[i]->Project(ray3D);
      // get distance vector to midpoint
      HomgPoint2D dist = edgepoint - midPoint;
      HomgPoint2D grad = edgepoint - oldPoint;

      // going right (or left)
      if (abs(grad[0]) > abs(grad[1])) {
        if (dist[1] < 0) {
          if (edgepoint[1] > ymin) {
            ymin = (unsigned)rint(edgepoint[1]);
          }
        } else {
          if (edgepoint[1] < ymax) {
            ymax = (unsigned)rint(edgepoint[1]);
          }
        }
      } // going up (or down)
      else {
        if (dist[0] < 0) {
          if (edgepoint[0] > xmin) {
            xmin = (unsigned)rint(edgepoint[0]);
          }
        } else {
          if (edgepoint[0] < xmax) {
            xmax = (unsigned)rint(edgepoint[0]);
          }
        }
      }

      // discard points not in image
      //      if (edgepoint[0] >= 0 && edgepoint[0] < w && edgepoint[1] >= 0 && edgepoint[1] < h) {
      //        ida[ (int)edgepoint[1] ][ (int)edgepoint[0] ] = 100;
      //      }
    } while (source[i]->GetNextBorderPixel(it));
  }
  //  ida [ ymin ] [ xmin ] = 255;
  //  ida [ ymin ] [ xmax ] = 255;
  //  ida [ ymax ] [ xmin ] = 255;
  //  ida [ ymax ] [ xmax ] = 255;
  roi.SetCorners(xmin, ymin, xmax, ymax);
  //  ImageIO::Save("test.mip", testIm);
  return 0;
}

/** it is assumed that the roi was calculated with a rescale factor of 1. **/
double CalculateRescaleFactor(int targetWidth, int targetHeight, int roiWidth,
                              int roiHeight) {

  BIASASSERT(targetHeight > 0 || targetWidth > 0);
  float scaleW = 0;
  float scaleH = 0;
  if (targetHeight > 0)
    scaleH = float(targetHeight) / float(roiHeight);
  if (targetWidth > 0)
    scaleW = float(targetWidth) / float(roiWidth);

  float scale = (scaleW > scaleH) ? scaleW : scaleH;

  return scale;
}

/** Will only work for planarily rectified images. **/
int findLargestOverlappingRegion(ProjectionParametersBase* camAproj,
                                 ProjectionParametersBase* camBproj,
                                 ProjectionParametersPerspective* rectAproj,
                                 ProjectionParametersPerspective* rectBproj,
                                 const double* zeroParallaxZ, ROI& roiA, ROI& roiB, bool verbose) {
  // cout<<"finding roiA:\n";
  findLargestRectangularRegion(camAproj, rectAproj, roiA);
  //  cout<<"finding roiB:\n";
  findLargestRectangularRegion(camBproj, rectBproj, roiB);

  //determine the zero parallax
  Vector3<double> B = rectBproj->GetC() - rectAproj->GetC();
  double baseline = B.NormL2();
  double f;
  rectAproj->GetFocalLength(f);
  double parallaxDispTmp = f * (baseline / *zeroParallaxZ);

  int dispZP = 0;
  if (*zeroParallaxZ >= 0)
    dispZP = static_cast<int> (rint(parallaxDispTmp));
  BIASASSERT(dispZP >= 0);

  if (verbose)
    cout << "disparity of Zero Parallax " << dispZP << endl;

  int rightUL[2];
  int rightLR[2];
  int leftUL[2];
  int leftLR[2];

  int commonUL[2];
  int commonLR[2];

  //now determine overlap in common coordinate frame:
  //shift right image for that
  if (B[0] > 0) {// cam B is the right
    rightUL[0] = roiB.GetCornerUpperLeftX() + dispZP;
    rightLR[0] = roiB.GetCornerLowerRightX() + dispZP;
    rightUL[1] = roiB.GetCornerUpperLeftY();
    rightLR[1] = roiB.GetCornerLowerRightY();

    leftUL[0] = roiA.GetCornerUpperLeftX();
    leftLR[0] = roiA.GetCornerLowerRightX();
    leftUL[1] = roiA.GetCornerUpperLeftY();
    leftLR[1] = roiA.GetCornerLowerRightY();

  } else {// cam A is the right
    rightUL[0] = roiA.GetCornerUpperLeftX() + dispZP;
    rightLR[0] = roiA.GetCornerLowerRightX() + dispZP;
    rightUL[1] = roiA.GetCornerUpperLeftY();
    rightLR[1] = roiA.GetCornerLowerRightY();

    leftUL[0] = roiB.GetCornerUpperLeftX();
    leftLR[0] = roiB.GetCornerLowerRightX();
    leftUL[1] = roiB.GetCornerUpperLeftY();
    leftLR[1] = roiB.GetCornerLowerRightY();

  }
  //now determine the overlap

  commonUL[0] = max(rightUL[0], leftUL[0]);
  commonUL[1] = max(rightUL[1], leftUL[1]);
  commonLR[0] = min(rightLR[0], leftLR[0]);
  commonLR[1] = min(rightLR[1], leftLR[1]);

  if (verbose) {
    cout << "identified left region : ";
    cout << "(" << leftUL[0] << "," << leftUL[1] << ")" << "<-->("
         << leftLR[0] << "," << leftLR[1] << ")\n";
    cout << "identified right region : ";
    cout << "(" << rightUL[0] << "," << rightUL[1] << ")" << "<-->("
         << rightLR[0] << "," << rightLR[1] << ")\n";

    cout << "identified common region : ";
    cout << "(" << commonUL[0] << "," << commonUL[1] << ")" << "<-->("
         << commonLR[0] << "," << commonLR[1] << ")\n";
  }

  // return the overlap into the left and right image's coordinate frame.
  if (B[0] > 0) {// cam B is the right

    roiB.SetCorners(commonUL[0] - dispZP, commonUL[1],
                    commonLR[0] - dispZP, commonLR[1]);

    roiA.SetCorners(commonUL[0], commonUL[1], commonLR[0], commonLR[1]);

  } else {// cam A is the right
    roiA.SetCorners(commonUL[0] - dispZP, commonUL[1],
                    commonLR[0] - dispZP, commonLR[1]);

    roiB.SetCorners(commonUL[0], commonUL[1], commonLR[0], commonLR[1]);

  }

  return 0;
}

void CentralCropRoi(int targetWidth, int targetHeight, ROI& roi) {
  //central crop to much roi
  int roiHeight = roi.GetCornerLowerRightY() - roi.GetCornerUpperLeftY();
  int roiWidth = roi.GetCornerLowerRightX() - roi.GetCornerUpperLeftX();

  int ul[2];
  int lr[2];
  int offsetWidth = 0;
  if (roiWidth > targetWidth) {
    offsetWidth = (roiWidth - targetWidth) / 2;
  }
  ul[0] = roi.GetCornerUpperLeftX() + offsetWidth;
  lr[0] = ul[0] + targetWidth;

  int offsetHeight = 0;
  if (roiHeight > targetHeight) {
    offsetHeight = (roiHeight - targetHeight) / 2;
  }
  ul[1] = roi.GetCornerUpperLeftY() + offsetHeight;
  lr[1] = ul[1] + targetHeight;

  roi.SetCorners(ul[0], ul[1], lr[0], lr[1]);
}