RectificationViaProjectionMappingBase.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 <Image/RectificationViaProjectionMappingBase.hh>

#include <Image/RectificationViaProjectionMappingBase.hh>
#include <Image/ProjectionMapping.hh>
#include <Geometry/Triangulation.hh>
#include <limits>

//#define validitydebug
#ifdef validitydebug
#include <Base/Image/ImageIO.hh>
#endif

// enable this for gradient based alignemnt of grey values along epipolar lines
//#define SUBPIXEL_REFINEMENT
#ifdef SUBPIXEL_REFINEMENT
# include <Matcher2D/TrackerBaseWeighted1D.hh>
# include <Base/Image/ImageConvert.hh>
# include <Filter/GradientSimple.hh>
# include <Filter/Gauss.hh>
# include <Base/Image/ImageIO.hh>
#endif

#define PARENT RectificationBase<InputStorageType, OutputStorageType>

using namespace BIAS;
using namespace std;

template<class InputStorageType, class OutputStorageType>
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::~RectificationViaProjectionMappingBase()
  {
    delete rectPPA_;
    delete rectPPB_;
  }

template<class InputStorageType, class OutputStorageType>
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::RectificationViaProjectionMappingBase(
      const std::string& stringID, const double& scale) :
    stringID_(stringID), rectPPA_(NULL), rectPPB_(NULL), scale_(scale),
        targetWidth_(0), rectificationParamsAreValid_(false),
        interpolationType_(MapTrilinear), mappingInitialized_(false),
        thetaMinOverride_(0), thetaMaxOverride_(M_PI), phiMinOverride_(-M_PI),
        phiMaxOverride_(M_PI), useLookUpTables_(true)
  {
    secondFill_ = std::numeric_limits<OutputStorageType>::max();
  }

template<class InputStorageType, class OutputStorageType>
  int
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::SetCameraA(BIAS::Camera<InputStorageType>& cam)
  {
    /* rectificationParamsAreValid_ = false;
     return PARENT::SetCameraA(cam);*/
    cam.ParseMetaData();
    return SetCameraA(cam, cam.GetProj().GetParameters());
  }

template<class InputStorageType, class OutputStorageType>
  int
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::SetCameraA(const BIAS::Image<InputStorageType>& img,
      const BIAS::ProjectionParametersBase* proj)
  {
    int res = PARENT::SetCameraA(img, proj);
    if (res == 0)
      {
        rectificationParamsAreValid_ = false;
      }
    return res;
  }

template<class InputStorageType, class OutputStorageType>
  int
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::SetCameraB(BIAS::Camera<InputStorageType>& cam)
  {
    //rectificationParamsAreValid_ = false;
    //return PARENT::SetCameraB(cam);
    cam.ParseMetaData();
    return SetCameraB(cam, cam.GetProj().GetParameters());
  }

template<class InputStorageType, class OutputStorageType>
  int
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::SetCameraB(const BIAS::Image<InputStorageType>& img,
      const BIAS::ProjectionParametersBase* proj)
  {
    int res = PARENT::SetCameraB(img, proj);
    if (res == 0)
      {
        rectificationParamsAreValid_ = false;
      }
    return res;
  }

template<class InputStorageType, class OutputStorageType>
  void
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::SetInterpolationMethod(
      InterpolationMethod interpolationType)
  {
    interpolationType_ = interpolationType;
    rectificationParamsAreValid_ = false;
  }

template<class InputStorageType, class OutputStorageType>
  bool
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::AreRectificationParametersValid_()
  {
    if (!rectificationParamsAreValid_)
      {
        if (DetermineScaledRectificationParameters_() == 0)
          {
            rectificationParamsAreValid_ = true;
            mappingInitialized_ = false;
          }
      }
    return rectificationParamsAreValid_;
  }

template<class InputStorageType, class OutputStorageType>
  int
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::DetermineScaledRectificationParameters_()
  {
    int res = DetermineRectificationParameters_();
    if (res == 0)
      {
          BIASASSERT(rectPPA_!=NULL);
          BIASASSERT(rectPPB_!=NULL);
        if (targetWidth_ > 0)
          {
            unsigned int width, height;
            rectPPA_->GetImageSize(width, height);
            float scale = float(width) / float(targetWidth_);
            rectPPA_->Rescale(float(1.0 / scale));

            rectPPB_->GetImageSize(width, height);
            scale = float(width) / float(targetWidth_);
            rectPPB_->Rescale(float(1.0 / scale));
          }
        else
          {
            rectPPA_->Rescale(float(1.0 / scale_));
            rectPPB_->Rescale(float(1.0 / scale_));
          }
      }

    return res;
  }

template<class InputStorageType, class OutputStorageType>
  void
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::InitializeSinkImages_()
  {
    //init sink image sizes and luts:
    unsigned int width, height;

    rectPPA_->GetImageSize(width, height);
    if (!PARENT::rectImageA_.IsEmpty())
      PARENT::rectImageA_.Release();
    PARENT::rectImageA_.Init(width, height, PARENT::imageA_.GetChannelCount());
    PARENT::rectImageA_.Clear();

    rectPPB_->GetImageSize(width, height);
    if (!PARENT::rectImageB_.IsEmpty())
      PARENT::rectImageB_.Release();
    PARENT::rectImageB_.Init(width, height, PARENT::imageB_.GetChannelCount());

  }

template<class InputStorageType, class OutputStorageType>
  void
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::InitializeProjectionMappingObjects_()
  {
    rectMapperA_.SetSourceCam(Projection(*PARENT::ppBA_));
    rectMapperA_.SetSinkCam(Projection(*rectPPA_));

    rectMapperB_.SetSourceCam(Projection(*PARENT::ppBB_));
    rectMapperB_.SetSinkCam(Projection(*rectPPB_));
  }

template<class InputStorageType, class OutputStorageType>
  void
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::InitializeMapping_()
  {
    if (mappingInitialized_)
      return;

    InitializeSinkImages_();
    InitializeProjectionMappingObjects_();

    if (useLookUpTables_)
      {
        rectMapperA_.PrepareLookupTableMapping(PARENT::imageA_,
            PARENT::rectImageA_, interpolationType_);

        rectMapperB_.PrepareLookupTableMapping(PARENT::imageB_,
            PARENT::rectImageB_, interpolationType_);
      }
    mappingInitialized_ = true;
  }

template<class InputStorageType, class OutputStorageType>
  int
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::GetClonesOfRectificationParameters(
      ProjectionParametersBase*& rectPPA, ProjectionParametersBase*& rectPPB)
  {
    rectPPA = NULL;
    rectPPB = NULL;

    if (!AreRectificationParametersValid_())
      {
        return -1;
      }

    if (rectPPA_ == NULL || rectPPB_ == NULL)
      {
        return -1;
      }

    rectPPA = rectPPA_->Clone();
    rectPPB = rectPPB_->Clone();
    return 0;
  }

template<class InputStorageType, class OutputStorageType>
  int
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::GetRectifyingDisplacementMaps(
      Image<float>& displacementA, Image<float>& displacementB)
  {
    if (!AreRectificationParametersValid_())
      {
        return -1;
      }
    InitializeProjectionMappingObjects_();

    unsigned int width, height;
    rectPPA_->GetImageSize(width, height);
    if (!displacementA.IsEmpty())
      displacementA.Release();
    displacementA.Init(width, height, 3);
    if (rectMapperA_.GetDisplacementMap(displacementA, PARENT::imageA_) != 0)
      {
        return -1;
      }

    rectPPB_->GetImageSize(width, height);
    if (!displacementB.IsEmpty())
      displacementB.Release();
    displacementB.Init(width, height, 3);
    if (rectMapperB_.GetDisplacementMap(displacementB, PARENT::imageB_) != 0)
      {
        return -1;
      }

    return 0;
  }

template<class InputStorageType, class OutputStorageType>
  int
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::GetRectificationSetup(Camera<float>& rectSetup)
  {

    if (!AreRectificationParametersValid_())
      {
        return -1;
      }

    InitializeMapping_();

    unsigned int width, channels;
    unsigned int widthB, channelsB;
    if (rectMapperA_.GetLookupTableSize(width, channels, interpolationType_)
        < 0)
      return -1;
    if (rectMapperB_.GetLookupTableSize(widthB, channelsB, interpolationType_)
        < 0)
      return -1;

      BIASASSERT(width == widthB);
      BIASASSERT(channels == channelsB);
    rectSetup.Init(width, 2, channels);

    //build projection here and set to metadata!
    //all data is relative -> this does not allow for classical
    // interpretation of the external cameras
    Projection Proj;
    Proj.AddRelativeCamera(PARENT::ppBA_);
    Proj.AddRelativeCamera(PARENT::ppBB_);
    Proj.AddRelativeCamera(rectPPA_);
    Proj.AddRelativeCamera(rectPPB_);
    rectSetup.SetProj(Proj);

    //add the id here!
    rectSetup.SetASCIIString(stringID_);

    rectSetup.UpdateMetaData();

    //set image content:
    float** rows = rectSetup.GetImageDataArray();
    if (rectMapperA_.GetLookupTable(rows[0], interpolationType_) < 0)
      return -1;
    if (rectMapperB_.GetLookupTable(rows[1], interpolationType_) < 0)
      return -1;

    return 0;
  }

template<class InputStorageType, class OutputStorageType>
  int
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::SetRectificationSetup(Camera<float>& rectSetup)
  {
    rectSetup.ParseMetaData();

    if (rectSetup.GetASCIIString() != stringID_)
      {
          BIASERR("loading rectification setup with wrong class");
        return -1;
      }

    Projection Proj = rectSetup.GetProj();
    PARENT::ppBA_ = Proj.GetParameters(0)->Clone();
    PARENT::ppBB_ = Proj.GetParameters(1)->Clone();
    rectPPA_ = Proj.GetParameters(2)->Clone();
    rectPPB_ = Proj.GetParameters(3)->Clone();

    float** rows = rectSetup.GetImageDataArray();
    InterpolationMethod method;
    rectMapperA_.SetLookupTable(rows[0], rectSetup.GetWidth(), method);
    rectMapperB_.SetLookupTable(rows[1], rectSetup.GetWidth(),
        interpolationType_);
      BIASASSERT(method == interpolationType_);

    InitializeSinkImages_();

    rectificationParamsAreValid_ = true;
    mappingInitialized_ = true;
    useLookUpTables_ = true;

    return 0;
  }

template<class InputStorageType, class OutputStorageType>
  void
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::SetOverrideAngles(double thetaMinOverride,
      double thetaMaxOverride, double phiMinOverride, double phiMaxOverride)
  {
      BIASASSERT(thetaMinOverride>=0 && thetaMinOverride<thetaMaxOverride);
      BIASASSERT(thetaMaxOverride>0 && thetaMaxOverride<=M_PI);
      BIASASSERT(phiMinOverride>=-M_PI && phiMinOverride<phiMaxOverride);
      BIASASSERT(phiMaxOverride>0 && phiMaxOverride<=M_PI);

    thetaMinOverride_ = thetaMinOverride;
    thetaMaxOverride_ = thetaMaxOverride;
    phiMinOverride_ = phiMinOverride;
    phiMaxOverride_ = phiMaxOverride;
    rectificationParamsAreValid_ = false;
  }

template<class InputStorageType, class OutputStorageType>
  int
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::Rectify()
  {
    if (!AreRectificationParametersValid_())
      {
        return -1;
      }

    InitializeMapping_();

    //clear images:
    PARENT::rectImageA_.SetZero();
    OutputStorageType* fill =
        new OutputStorageType[PARENT::imageB_.GetChannelCount()];
    for (unsigned int i = 0; i < PARENT::imageB_.GetChannelCount(); i++)
      {
        fill[i] = secondFill_;
      }
    PARENT::rectImageB_.FillImageWithConstValue(*fill);
    delete[] fill;

    if (useLookUpTables_)
      {
        if (rectMapperA_.MapWithLookupTable(PARENT::imageA_,
            PARENT::rectImageA_, interpolationType_) < 0)
          {
              BIASERR("LUT rectification of image A failed");
            return -1;
          }

        if (rectMapperB_.MapWithLookupTable(PARENT::imageB_,
            PARENT::rectImageB_, interpolationType_) < 0)
          {
              BIASERR("LUT rectification of image B failed");
            return -1;
          }
      }
    else
      {
        if (rectMapperA_.Map(PARENT::imageA_, PARENT::rectImageA_,
            interpolationType_) < 0)
          {
              BIASERR("rectification of image A failed");
            return -1;
          }

        if (rectMapperB_.Map(PARENT::imageB_, PARENT::rectImageB_,
            interpolationType_) < 0)
          {
              BIASERR("rectification of image B failed");
            return -1;
          }
      }

    return 0;
  }

template<class InputStorageType, class OutputStorageType>
  int
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::Disp2Depth(const Image<float>& DisparityMap, Image<
      float>& DepthMap, const ProjectionParametersBase* ppBA,
      const ProjectionParametersBase* ppBB, const ProjectionParametersBase* rectPPA,
      const ProjectionParametersBase* rectPPB, unsigned int border)
  {
    unsigned int width, height;
    ppBA->GetImageSize(width, height);

    if (!DepthMap.IsEmpty())
      DepthMap.Release();
    DepthMap.Init(width, height);
    DepthMap.SetZero();

#ifdef validitydebug
    Image<unsigned char> ValidityMap;
    ValidityMap.Init(width, height);
    ValidityMap.SetZero();
    unsigned char** ValidityMapPel =
    ValidityMap.GetImageDataArray();
#endif

    float** DepthMapPel = DepthMap.GetImageDataArray();
    //  float** DispMapPel = DisparityMap.GetImageDataArray();
    //  unsigned int dispMapWidth = DisparityMap.GetWidth();
    //  unsigned int dispMapHeight = DisparityMap.GetHeight();

    float disparityThreshold = 0.0001f
        + (float) DisparityMap.GetMinPixelValue();

    //allows acces from Orig A to Rect A via GetSourceCoords
    ProjectionMapping<InputStorageType, OutputStorageType> relativeOrigAToRectA;
    //allows acces from Rect B to Orig B via GetSourceCoords
    ProjectionMapping<InputStorageType, OutputStorageType> relativeRectBToOrigB;

    relativeOrigAToRectA.SetSinkCam(Projection(*ppBA));
    relativeOrigAToRectA.SetSourceCam(Projection(*rectPPA));
    relativeRectBToOrigB.SetSinkCam(Projection(*rectPPB));
    relativeRectBToOrigB.SetSourceCam(Projection(*ppBB));

#ifdef SUBPIXEL_REFINEMENT

    // want to track in float to avoid messing around with templates
    // compute blurred texture and gradient images in float:
    Gauss<OutputStorageType, float> Blurrer;
    Blurrer.SetSigma(0.1);
    Image<float> rectAf, gradax, graday;
    Blurrer.Filter(this->rectImageA_, rectAf);
    Image<float> rectBf, gradbx, gradby;
    Blurrer.Filter(this->rectImageB_, rectBf);
#ifdef BIAS_DEBUG_KK
    //ImageIO::Save("rectAf.mip", rectAf);
    //ImageIO::Save("rectBf.mip", rectBf);
    ImageIO::Save("rectAf.mip", rectAf);
    ImageIO::Save("rectBf.mip", rectBf);
#endif
    // assume that images are already blurred
    GradientSimple<float, float> MyGradient;
    MyGradient.Filter(rectAf, gradax, graday);
    MyGradient.Filter(rectBf, gradbx, gradby);
#ifdef BIAS_DEBUG_KK
    //ImageIO::Save("gradax.mip", gradax);
    //ImageIO::Save("gradbx.mip", gradbx);
    ImageIO::Save("gradax.mip", gradax);
    ImageIO::Save("gradbx.mip", gradbx);
#endif

    // generate tracker which searches only for dx
    TrackerBaseWeighted1D<float> SubPixelRefiner;
    //SubPixelRefiner.SetDebugLevel(D_TRACKERB_KLT);
    SubPixelRefiner.SetHalfWinSize(7);
    SubPixelRefiner.SetMaxIterations(25);
    SubPixelRefiner.SetMaxError(0.05);
    SubPixelRefiner.SetMaxResiduumMAD(1e10);
    SubPixelRefiner.SetRejectionType(RT_MAD);
    SubPixelRefiner.SetAffineBrightnessInvariance(true);
    SubPixelRefiner.Init(rectAf, rectBf, gradax, graday, gradbx, gradby);

    // some variables which we need but which we wont evaluate ...
    KLT_TYPE kltsteperror = 0;
    int kltiterations = 0;
    KLT_TYPE residuumMAD = 0;
    KLT_TYPE residuumMSD = 0;
    Matrix<KLT_TYPE> cov(2,2,MatrixZero);
#endif

    HomgPoint2D depthMapPoint, dispMapPoint;
    HomgPoint2D correspondenceInRectB, correspondenceInOrigB;
    HomgPoint3D point3D;

    Triangulation triangulator;
    double disparity = 0;

    depthMapPoint[2] = 1.0;
    dispMapPoint[2] = 1.0;
    for (unsigned int y = border; y < height - border; y++)
      {
        for (unsigned int x = border; x < width - border; x++)
          {

            depthMapPoint[0] = x;
            depthMapPoint[1] = y;

            bool valid = true;
            if (relativeOrigAToRectA.GetSourceCoordinates(depthMapPoint,
                dispMapPoint) == 0)
              {
                //have rect A coords get me the disp map value
                disparity = DisparityMap.BilinearInterpolationGreyMinVal(
                    dispMapPoint[0], dispMapPoint[1], disparityThreshold);
                if (disparity > disparityThreshold)
                  { //valid disparity
                    correspondenceInRectB.Set(dispMapPoint[0] - disparity,
                        dispMapPoint[1], 1.0);
                    // dont let the refiner compute an offset larger than this
#define MAX_OFFSET_REFINEMENT 0.6
#ifdef SUBPIXEL_REFINEMENT
                    HomgPoint2D refinedPoint = correspondenceInRectB;

                    // track only those points which are triangulated


                    // Instead of using a constant gaussian weighted window for every
                    // correspondence, it would be much more intelligent to use
                    // something like 1/(0.5+fabs(disparity difference)) as a weight for
                    // each pixel in the neighborhood. This automatically decreases
                    // the window at non-fronto-parallel structures or depth
                    // discontinuities.
                    // simply add a SetWeight method to Tracker1D


                    int refinementresult =
                    SubPixelRefiner.Track(dispMapPoint, correspondenceInRectB,
                        refinedPoint,
                        kltsteperror, kltiterations,residuumMAD,
                        residuumMSD, cov);

                    // did the refinement succeed at all ?
                    if (refinementresult==0)
                      {
                        // fine, check if the step is consistent with our guess or
                        // whether it is too large (violates smoothness in neighborhood)
                        double diff = refinedPoint[0]-correspondenceInRectB[0];
                        // make sure y does not change
                        BIASASSERT(Equal(refinedPoint[1], correspondenceInRectB[1]));
                        if (fabs(diff)<MAX_OFFSET_REFINEMENT)
                          {
                            // fine! set this as the new correspondence for the point
                            correspondenceInRectB = refinedPoint;
                            //cout<<"+("<<diff<<") ";
                          }
                        else
                          {
                            // step to big, presumably other local minimum
                            //cout<<"-("<<diff<<") ";
                          }
                      }
                    else
                      {
                        // klt complained, presumably no structure or too many iterations
                        //cout<<"!"<<refinementresult<<"!";
                      }
#endif

                    if (relativeRectBToOrigB. GetSourceCoordinates(
                        correspondenceInRectB, correspondenceInOrigB) == 0)
                      {

                        //have got correspondance in orig B!
                        if (triangulator.Triangulate(ppBA, ppBB, depthMapPoint,
                            correspondenceInOrigB, point3D) == 0)
                          {
                            //ok triangulation was good calc depth
                            Vector3<double> euc;
                            point3D.GetEuclidean(euc);
                            Vector3<double> diff = euc - ppBA->GetC();

                              BIASASSERT(diff.NormL2()>1e-5);
                            DepthMapPel[y][x] = (float) diff.NormL2();
                            valid = true;
                          }
                      } //no correspondance in orig B :(
                    else
                      valid = false;
                  }//disprity is too small:(
                else
                  valid = false;

              } //end of check if rect coord available
            else
              valid = false;

            if (!valid)
              {//ok did we get valid depth ?
                DepthMapPel[y][x] = UNDEFINED_DEPTH;
#ifdef validitydebug
                ValidityMapPel[y][x] = 0;
#endif
              }
#ifdef validitydebug
            else
            ValidityMapPel[y][x] = 255;
#endif

          }//end of for x
#ifdef SUBPIXEL_REFINEMENT
        // a new line is completed
        cout<<"S"<<flush;
#endif
      }//end of for y
#ifdef validitydebug
    //ImageIO::Save("D2DValidityMap.mip", ValidityMap);
    ImageIO::Save("D2DValidityMap.mip", ValidityMap);
#endif

    return 0;
  }

template<class InputStorageType, class OutputStorageType>
  int
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::Disp2Depth(const Image<float>& DisparityMap, Image<
      float>& DepthMap, unsigned int border)
  {
    if (!AreRectificationParametersValid_())
      {
        return -1;
      }

    RectificationViaProjectionMappingBase<unsigned char, unsigned char>::Disp2Depth(
        DisparityMap, DepthMap, PARENT::ppBA_, PARENT::ppBB_, rectPPA_,
        rectPPB_, border);

    return 0;

    //    if (!AreRectificationParametersValid_())
    //      {
    //        return -1;
    //      }
    //
    //    unsigned int width, height;
    //    PARENT::ppBA_->GetImageSize(width, height);
    //
    //    if (!DepthMap.IsEmpty())
    //      DepthMap.Release();
    //    DepthMap.Init(width, height);
    //    DepthMap.SetZero();
    //
    //#ifdef validitydebug
    //    Image<unsigned char> ValidityMap;
    //    ValidityMap.Init(width, height);
    //    ValidityMap.SetZero();
    //    unsigned char** ValidityMapPel =
    //    ValidityMap.GetImageDataArray();
    //#endif
    //
    //    float** DepthMapPel = DepthMap.GetImageDataArray();
    //    //  float** DispMapPel = DisparityMap.GetImageDataArray();
    //    //  unsigned int dispMapWidth = DisparityMap.GetWidth();
    //    //  unsigned int dispMapHeight = DisparityMap.GetHeight();
    //
    //    float disparityThreshold = 0.0001f
    //        + (float) DisparityMap.GetMinPixelValue();
    //
    //    //allows acces from Orig A to Rect A via GetSourceCoords
    //    ProjectionMapping<InputStorageType, OutputStorageType> relativeOrigAToRectA;
    //    //allows acces from Rect B to Orig B via GetSourceCoords
    //    ProjectionMapping<InputStorageType, OutputStorageType> relativeRectBToOrigB;
    //
    //    relativeOrigAToRectA.SetSinkCam(Projection(*PARENT::ppBA_));
    //    relativeOrigAToRectA.SetSourceCam(Projection(*rectPPA_));
    //    relativeRectBToOrigB.SetSinkCam(Projection(*rectPPB_));
    //    relativeRectBToOrigB.SetSourceCam(Projection(*PARENT::ppBB_));
    //
    //#ifdef SUBPIXEL_REFINEMENT
    //
    //    // want to track in float to avoid messing around with templates
    //    // compute blurred texture and gradient images in float:
    //    Gauss<OutputStorageType, float> Blurrer;
    //    Blurrer.SetSigma(0.1);
    //    Image<float> rectAf, gradax, graday;
    //    Blurrer.Filter(this->rectImageA_, rectAf);
    //    Image<float> rectBf, gradbx, gradby;
    //    Blurrer.Filter(this->rectImageB_, rectBf);
    //#ifdef BIAS_DEBUG_KK
    //    ImageIO::Save("rectAf.mip", rectAf);
    //    ImageIO::Save("rectBf.mip", rectBf);
    //#endif
    //    // assume that images are already blurred
    //    GradientSimple<float, float> MyGradient;
    //    MyGradient.Filter(rectAf, gradax, graday);
    //    MyGradient.Filter(rectBf, gradbx, gradby);
    //#ifdef BIAS_DEBUG_KK
    //    ImageIO::Save("gradax.mip", gradax);
    //    ImageIO::Save("gradbx.mip", gradbx);
    //#endif
    //
    //    // generate tracker which searches only for dx
    //    TrackerBaseWeighted1D<float> SubPixelRefiner;
    //    //SubPixelRefiner.SetDebugLevel(D_TRACKERB_KLT);
    //    SubPixelRefiner.SetHalfWinSize(7);
    //    SubPixelRefiner.SetMaxIterations(25);
    //    SubPixelRefiner.SetMaxError(0.05);
    //    SubPixelRefiner.SetMaxResiduumMAD(1e10);
    //    SubPixelRefiner.SetRejectionType(RT_MAD);
    //    SubPixelRefiner.SetAffineBrightnessInvariance(true);
    //    SubPixelRefiner.Init(rectAf, rectBf, gradax, graday, gradbx, gradby);
    //
    //    // some variables which we need but which we wont evaluate ...
    //    KLT_TYPE kltsteperror = 0;
    //    int kltiterations = 0;
    //    KLT_TYPE residuumMAD = 0;
    //    KLT_TYPE residuumMSD = 0;
    //    Matrix<KLT_TYPE> cov(2,2,MatrixZero);
    //#endif
    //
    //    HomgPoint2D depthMapPoint, dispMapPoint;
    //    HomgPoint2D correspondenceInRectB, correspondenceInOrigB;
    //    HomgPoint3D point3D;
    //
    //    Triangulation triangulator;
    //    double disparity = 0;
    //
    //    depthMapPoint[2] = 1.0;
    //    dispMapPoint[2] = 1.0;
    //    for (unsigned int y = border; y < height - border; y++)
    //      {
    //        for (unsigned int x = border; x < width - border; x++)
    //          {
    //
    //            depthMapPoint[0] = x;
    //            depthMapPoint[1] = y;
    //
    //            bool valid = true;
    //            if (relativeOrigAToRectA.GetSourceCoordinates(depthMapPoint,
    //                dispMapPoint) == 0)
    //              {
    //                //have rect A coords get me the disp map value
    //                disparity = DisparityMap.BilinearInterpolationGreyMinVal(
    //                    dispMapPoint[0], dispMapPoint[1], disparityThreshold);
    //                if (disparity > disparityThreshold)
    //                  { //valid disparity
    //                    correspondenceInRectB.Set(dispMapPoint[0] - disparity,
    //                        dispMapPoint[1], 1.0);
    //                    // dont let the refiner compute an offset larger than this
    //#define MAX_OFFSET_REFINEMENT 0.6
    //#ifdef SUBPIXEL_REFINEMENT
    //                    HomgPoint2D refinedPoint = correspondenceInRectB;
    //
    //                    // track only those points which are triangulated
    //
    //
    //                    // Instead of using a constant gaussian weighted window for every
    //                    // correspondence, it would be much more intelligent to use
    //                    // something like 1/(0.5+fabs(disparity difference)) as a weight for
    //                    // each pixel in the neighborhood. This automatically decreases
    //                    // the window at non-fronto-parallel structures or depth
    //                    // discontinuities.
    //                    // simply add a SetWeight method to Tracker1D
    //
    //
    //                    int refinementresult =
    //                    SubPixelRefiner.Track(dispMapPoint, correspondenceInRectB,
    //                        refinedPoint,
    //                        kltsteperror, kltiterations,residuumMAD,
    //                        residuumMSD, cov);
    //
    //                    // did the refinement succeed at all ?
    //                    if (refinementresult==0)
    //                      {
    //                        // fine, check if the step is consistent with our guess or
    //                        // whether it is too large (violates smoothness in neighborhood)
    //                        double diff = refinedPoint[0]-correspondenceInRectB[0];
    //                        // make sure y does not change
    //                        BIASASSERT(Equal(refinedPoint[1], correspondenceInRectB[1]));
    //                        if (fabs(diff)<MAX_OFFSET_REFINEMENT)
    //                          {
    //                            // fine! set this as the new correspondence for the point
    //                            correspondenceInRectB = refinedPoint;
    //                            //cout<<"+("<<diff<<") ";
    //                          }
    //                        else
    //                          {
    //                            // step to big, presumably other local minimum
    //                            //cout<<"-("<<diff<<") ";
    //                          }
    //                      }
    //                    else
    //                      {
    //                        // klt complained, presumably no structure or too many iterations
    //                        //cout<<"!"<<refinementresult<<"!";
    //                      }
    //#endif
    //
    //                    if (relativeRectBToOrigB. GetSourceCoordinates(
    //                        correspondenceInRectB, correspondenceInOrigB) == 0)
    //                      {
    //
    //                        //have got correspondance in orig B!
    //                        if (triangulator.Triangulate(PARENT::ppBA_,
    //                            PARENT::ppBB_, depthMapPoint,
    //                            correspondenceInOrigB, point3D) == 0)
    //                          {
    //                            //ok triangulation was good calc depth
    //                            Vector3<double> euc;
    //                            point3D.GetEuclidean(euc);
    //                            Vector3<double> diff = euc - PARENT::ppBA_->GetC();
    //
    //                              BIASASSERT(diff.NormL2()>1e-5);
    //                            DepthMapPel[y][x] = (float) diff.NormL2();
    //                            valid = true;
    //                          }
    //                      } //no correspondance in orig B :(
    //                    else
    //                      valid = false;
    //                  }//disprity is too small:(
    //                else
    //                  valid = false;
    //
    //              } //end of check if rect coord available
    //            else
    //              valid = false;
    //
    //            if (!valid)
    //              {//ok did we get valid depth ?
    //                DepthMapPel[y][x] = UNDEFINED_DEPTH;
    //#ifdef validitydebug
    //                ValidityMapPel[y][x] = 0;
    //#endif
    //              }
    //#ifdef validitydebug
    //            else
    //            ValidityMapPel[y][x] = 255;
    //#endif
    //
    //          }//end of for x
    //#ifdef SUBPIXEL_REFINEMENT
    //        // a new line is completed
    //        cout<<"S"<<flush;
    //#endif
    //      }//end of for y
    //#ifdef validitydebug
    //    ImageIO::Save("D2DValidityMap.mip", ValidityMap);
    //#endif
    //
  }

template<class InputStorageType, class OutputStorageType>
  void
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::SetTargetWidth(unsigned int targetWidth)
  {
    targetWidth_ = targetWidth;
  }

template<class InputStorageType, class OutputStorageType>
  void
  BIAS::RectificationViaProjectionMappingBase<InputStorageType,
      OutputStorageType>::UseLookUpTables(bool useLUT)
  {
    //toggling of LUT mapping should only trigger a new initialization
    //of the mapping structures if LUT was not active.
    //For simple mapping the necessary init does depend on other
    //interfaces only.
    if (useLUT && !useLookUpTables_)
      mappingInitialized_ = false;
    useLookUpTables_ = useLUT;
  }

namespace BIAS{
template class RectificationViaProjectionMappingBase<unsigned char,
    unsigned char> ;
template class RectificationViaProjectionMappingBase<float, float> ;
template class RectificationViaProjectionMappingBase<unsigned char, float> ;
template class RectificationViaProjectionMappingBase<float, unsigned char> ;

#if defined(BUILD_IMAGE_CHAR)
template class RectificationViaProjectionMappingBase<unsigned char, char> ;
template class RectificationViaProjectionMappingBase<char, char> ;
#endif

#if defined(BUILD_IMAGE_USHORT)
template class RectificationViaProjectionMappingBase<unsigned short,
    unsigned short> ;
#endif

#if defined(BUILD_IMAGE_SHORT)
template class RectificationViaProjectionMappingBase<short, short> ;
#endif

#if defined(BUILD_IMAGE_SHORT)&&defined(BUILD_IMAGE_USHORT)
template class RectificationViaProjectionMappingBase<unsigned short, short> ;
#endif

#if defined(BUILD_IMAGE_INT)
template class RectificationViaProjectionMappingBase<int, int> ;
#endif

#if defined(BUILD_IMAGE_USHORT)
template class RectificationViaProjectionMappingBase<unsigned short, float> ;
#endif

#if defined(BUILD_IMAGE_USHORT) && defined(BUILD_IMAGE_INT)
template class RectificationViaProjectionMappingBase<unsigned short, int> ;
#endif

#if defined(BUILD_IMAGE_DOUBLE)
template class RectificationViaProjectionMappingBase<double, double> ;
#endif
}