PlanarRectification.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/RectificationBase.hh>
#include <Image/PlanarRectification.hh>
#include <Geometry/ProjectionParametersPerspective.hh>

using namespace BIAS;
using namespace std;

#define SUPERPARENT RectificationBase<InputStorageType, OutputStorageType>
#define PARENT RectificationViaProjectionMappingBase<InputStorageType, OutputStorageType>


template <class InputStorageType, class OutputStorageType>
BIAS::PlanarRectification<InputStorageType, OutputStorageType>::
PlanarRectification():
  PARENT::RectificationViaProjectionMappingBase("PlanarRectification"),
  rectOrientationGiven_(false),
  fixedIntrinsicsGiven_(false)
{ }

template <class InputStorageType, class OutputStorageType>
BIAS::PlanarRectification<InputStorageType, OutputStorageType>::
~PlanarRectification()
{ }

template <class InputStorageType, class OutputStorageType>
bool BIAS::PlanarRectification<InputStorageType, OutputStorageType>::
IsInputCameraValid(const BIAS::Image<InputStorageType>& img,
                   const BIAS::ProjectionParametersBase* proj)
{
  const ProjectionParametersPerspective* ppp =
    dynamic_cast<const ProjectionParametersPerspective*>(proj);

  if (ppp == NULL) {
    return false;
  }

  return true;
}

template <class InputStorageType, class OutputStorageType>
void BIAS::PlanarRectification<InputStorageType, OutputStorageType>::
FixateRectOrientation(const Quaternion<double>& orientation)
{
  rectOrientation_ = orientation;
  rectOrientationGiven_ = true;
  PARENT::rectificationParamsAreValid_ = false;
}

template <class InputStorageType, class OutputStorageType>
void BIAS::PlanarRectification<InputStorageType, OutputStorageType>::
FixateIntrinsics(const ProjectionParametersPerspective& ppp)
{
  fixedIntrinsicsA_ = fixedIntrinsicsB_ = ppp;
  fixedIntrinsicsGiven_ = true;
  PARENT::rectificationParamsAreValid_ = false;
}

template <class InputStorageType, class OutputStorageType>
void BIAS::PlanarRectification<InputStorageType, OutputStorageType>::
FixateIntrinsics(const ProjectionParametersPerspective& pppA,
                 const ProjectionParametersPerspective& pppB)
{
  fixedIntrinsicsA_ = pppA;
  fixedIntrinsicsB_ = pppB;
  fixedIntrinsicsGiven_ = true;
  PARENT::rectificationParamsAreValid_ = false;
}

template <class InputStorageType, class OutputStorageType>
int BIAS::PlanarRectification<InputStorageType, OutputStorageType>::
DetermineRectificationParameters_()
{

  Pose rectPoseA, rectPoseB;//, relativeTransformA, relativeTransformB;

  // simple test if rectification is necessary. fkellner
  if (!SUPERPARENT::ppBA_->DoIntrinsicsDiffer(SUPERPARENT::ppBB_)) {
    if (SUPERPARENT::ppBA_->GetQ().Dist(SUPERPARENT::ppBB_->GetQ()) == 0.0f)  {
      BIASWARN("The camera parameters look like the image pair is already "
               "rectified! Parameters are cloned.");
      PARENT::rectPPA_ = SUPERPARENT::ppBA_->Clone();
      PARENT::rectPPB_ = SUPERPARENT::ppBB_->Clone();
      return 0;
    }
  }

  if(!rectOrientationGiven_) {
    BIASASSERT((SUPERPARENT::ppBA_)!=NULL);
    BIASASSERT((SUPERPARENT::ppBB_)!=NULL);
    if (SUPERPARENT::CalculateRectifiedBases(SUPERPARENT::ppBA_->GetPose(),
                                             SUPERPARENT::ppBB_->GetPose(),
                                             rectPoseA, rectPoseB, true) != 0) {
      BIASERR("Failed to compute rectified base! Is epipole in any image?");
      return -1;
    }
  } else {
    Vector3<double> cA = SUPERPARENT::ppBA_->GetC();
    Vector3<double> cB = SUPERPARENT::ppBB_->GetC();
    rectPoseA.Set(rectOrientation_,cA);
    rectPoseB.Set(rectOrientation_, cB);
  }

  delete PARENT::rectPPA_;
  delete PARENT::rectPPB_;
  PARENT::rectPPA_ = new ProjectionParametersPerspective();
  PARENT::rectPPB_ = new ProjectionParametersPerspective();

  ProjectionParametersPerspective* ppPA_ =
    dynamic_cast<ProjectionParametersPerspective*> (SUPERPARENT::ppBA_);
  BIASASSERT(ppPA_ != NULL);
  ProjectionParametersPerspective* ppPB_ =
    dynamic_cast<ProjectionParametersPerspective*> (SUPERPARENT::ppBB_);
  BIASASSERT(ppPB_ != NULL);

  ProjectionParametersPerspective* rectPPPA_ =
    dynamic_cast<ProjectionParametersPerspective*> (PARENT::rectPPA_);
  ProjectionParametersPerspective* rectPPPB_ =
    dynamic_cast<ProjectionParametersPerspective*> (PARENT::rectPPB_);


  if(!fixedIntrinsicsGiven_) {
    //the whole thing should be optimized/corrected! :
    //KMatrix Ka = ppPA_->GetK();
    //Ka[0][1] = 0;

    ////assume that images have very low roll component relative to each other
    //// and that baseline direction is basically in direction of image width:
    //unsigned int widthA, heightA;
    //ppPA_->GetImageSize(widthA, heightA);
    //unsigned int widthB, heightB;
    //ppPB_->GetImageSize(widthB, heightB);
    //int numSamplesOnEpiLine = (widthA>widthB) ? widthA : widthB;
    //int numEpiLines = (heightA>heightB) ? heightA : heightB;

    //PARENT::rectPPA_->SetImageSize((unsigned int)numSamplesOnEpiLine,
    //                               (unsigned int)numEpiLines);
    //PARENT::rectPPB_->SetImageSize((unsigned int)numSamplesOnEpiLine,
    //                               (unsigned int)numEpiLines);

    //rectPPPA_->SetK(Ka);
    //rectPPPB_->SetK(Ka);

    
//     cout<<"orig R A: "<<ppPA_->GetR()<<endl;
//     cout<<"rect R A: "<<rectPoseA.GetR()<<endl;
//     cout<<"orig R B: "<<ppPB_->GetR()<<endl;
//     cout<<"rect R B: "<<rectPoseB.GetR()<<endl;

    //It can be assumed that the epipoles are not in the images!
    //Determine the spherical angles in rectified frames.
    CoordinateTransform3D spherePoseA = rectPoseA;
    spherePoseA.RotateLocalFrame(Quaternion<double>(0, -sin(M_PI_4),
                                                    0, cos(M_PI_4)));
    CoordinateTransform3D spherePoseB = rectPoseB;
    spherePoseB.RotateLocalFrame(Quaternion<double>(0, -sin(M_PI_4),
                                                    0, cos(M_PI_4)));

    double anglesA[5];
    double stepSizeA;
    ppPA_->GetSphericalViewingRange(spherePoseA, anglesA[0], anglesA[1],
                                    anglesA[2], anglesA[3], anglesA[4]);
    ppPA_->GetMinimalAngularSamplingStep(stepSizeA);

//     for (unsigned int i = 0; i < 5; i++) {
//       cout << "- anglesA[" << i << "] = " << anglesA[i]*180.0/M_PI
//            << " degree" << endl;
//     }


    double anglesB[5];
    double stepSizeB;
    ppPB_->GetSphericalViewingRange(spherePoseB, anglesB[0], anglesB[1],
                                    anglesB[2], anglesB[3], anglesB[4]);
    ppPB_->GetMinimalAngularSamplingStep(stepSizeB);

//     for(unsigned int i = 0; i < 5; i++) {
//       cout << "- anglesB[" << i << "] = " << anglesB[i]*180.0/M_PI
//            << " degree" << endl;
//     }




    //find common viewing field, exploit that B lies in positive x direction
    double minPhi = (anglesA[0]<anglesB[0]) ? anglesB[0] : anglesA[0];
    double maxPhi = (anglesA[1]>anglesB[1]) ? anglesB[1] : anglesA[1];
    //is the one we should always choose for the smaller angle
    double minTheta = anglesB[3];
    //is the one we should always choose for the larger angle
    double maxTheta = anglesA[4];
    //find resolution
    double minAngleStep = (stepSizeA<stepSizeB) ? stepSizeA : stepSizeB;

    double aspectratio = 1.0;

    if(retainAspectRatio_) {
      //calculate the means aspect ratio of original images:
      double aspectratio = 
        ppPA_->GetAspectratio() + 
        ppPB_->GetAspectratio();
      aspectratio /= 2.0;
      if(aspectratio<1.0) {
        minAngleStep *= aspectratio; 
        //do this to keep the minimal sampling step overall
      }
      
    }

//      spherePoseA = ppPA_->GetPose();
//     spherePoseA.RotateLocalFrame(Quaternion<double>(0, -sin(M_PI_4),
//                                                     0, cos(M_PI_4)));
//     spherePoseB = ppPB_->GetPose();
//     spherePoseB.RotateLocalFrame(Quaternion<double>(0, -sin(M_PI_4),
//                                                     0, cos(M_PI_4)));
//     ppPA_->GetSphericalViewingRange(spherePoseA, anglesA[0], anglesA[1],
//                                     anglesA[2], anglesA[3], anglesA[4]);
//     ppPA_->GetMinimalAngularSamplingStep(stepSizeA);

//     for (unsigned int i = 0; i < 5; i++) {
//       cout << "- orig anglesA[" << i << "] = " << anglesA[i]*180.0/M_PI
//            << " degree" << endl;
//     }
//     ppPB_->GetSphericalViewingRange(spherePoseB, anglesB[0], anglesB[1],
//                                     anglesB[2], anglesB[3], anglesB[4]);
//     ppPB_->GetMinimalAngularSamplingStep(stepSizeB);

//     for(unsigned int i = 0; i < 5; i++) {
//       cout << "- orig anglesB[" << i << "] = " << anglesB[i]*180.0/M_PI
//            << " degree" << endl;
//     }


 

    //set intrinsics
    rectPPPA_->SetIntrinsics(minPhi, maxPhi, minTheta, maxTheta, 
                             minAngleStep, aspectratio);
    rectPPPB_->SetIntrinsics(minPhi, maxPhi, minTheta, maxTheta, 
                             minAngleStep, aspectratio);

  } else {
    (*rectPPPA_) = fixedIntrinsicsA_;
    (*rectPPPB_) = fixedIntrinsicsB_;
  }

  rectPPPA_->SetPose(rectPoseA);
  rectPPPB_->SetPose(rectPoseB);


  return 0;
}


template <class InputStorageType, class OutputStorageType>
void BIAS::PlanarRectification<InputStorageType, OutputStorageType>::
RetainOriginalApectRatio(bool retain) 
{
  retainAspectRatio_ = retain;
}


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

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

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

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

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

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

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

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

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