RectificationBase.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 <Geometry/Projection.hh>

using namespace BIAS;
using namespace std;

template <class InputStorageType, class OutputStorageType>
BIAS::RectificationBase<InputStorageType, OutputStorageType>:: 
~RectificationBase()
{
  delete ppBA_;
  delete ppBB_;
}

template <class InputStorageType, class OutputStorageType>
BIAS::RectificationBase<InputStorageType, OutputStorageType>:: 
RectificationBase() : ppBA_(NULL), ppBB_(NULL)
{ }

template <class InputStorageType, class OutputStorageType>
bool BIAS::RectificationBase<InputStorageType, OutputStorageType>::
IsInputCameraValid(BIAS::Camera<InputStorageType>& cam)
{
  cam.ParseMetaData();
  return IsInputCameraValid(cam, cam.GetProj().GetParameters());
}


template <class InputStorageType, class OutputStorageType>
int BIAS::RectificationBase<InputStorageType, OutputStorageType>:: 
SetCameraA(BIAS::Camera<InputStorageType>& cam) 
{
 /* if (!IsInputCameraValid(cam)) {
    return -1;
  }*/

  cam.ParseMetaData();
  //Projection P = cam.GetProj();
  return SetCameraA(cam, cam.GetProj().GetParameters());

  /*delete ppBA_;
  ppBA_ = NULL;

  if (P.GetParameters() == NULL) {
    return -1;
  } else {
    ppBA_ = P.GetParameters()->Clone();
  }

  if (cam.IsEmpty()) {
    return -1;
  }

  imageA_ = cam;  

  return 0;*/
}


template <class InputStorageType, class OutputStorageType>
int BIAS::RectificationBase<InputStorageType, OutputStorageType>:: 
SetCameraA(const BIAS::Image<InputStorageType>& img,
           const BIAS::ProjectionParametersBase* proj) 
{
  if (!IsInputCameraValid(img, proj)) {
    BIASERR("input camera is invalid!");
    return -1;
  }
  int res = 0;

  bool setProjection = false;
  if(ppBA_ == NULL) {
    setProjection = true;
  } else {
    setProjection = (ppBA_->DoExtrinsicsDiffer(proj) || ppBA_->DoIntrinsicsDiffer(proj));
  }

  if(setProjection) 
  {
    delete ppBA_;
    ppBA_ = NULL;

    if (proj == NULL) {
      BIASERR("given projection is empty!")
      return -1;
    }

    ppBA_ = proj->Clone();
  } else {
    res = 1;
  }

  if (img.IsEmpty()) {
    return -1;
  }

  imageA_ = Camera<InputStorageType>(img);  

  return res;
}


template <class InputStorageType, class OutputStorageType>
int BIAS::RectificationBase<InputStorageType, OutputStorageType>:: 
SetCameraB(BIAS::Camera<InputStorageType>& cam) 
{

  cam.ParseMetaData();
  Projection P = cam.GetProj();
  return SetCameraB(cam, P.GetParameters());

//  if (!IsInputCameraValid(cam)) {
//    return -1;
//  }
//
////cam.ParseMetaData();
//  Projection P = cam.GetProj();
//
//  delete ppBB_;
//  ppBB_ = NULL;
//
//  if (P.GetParameters() == NULL) {
//    return -1;
//  } else {
//    ppBB_ = P.GetParameters()->Clone();
//  }
//
//  if (cam.IsEmpty()) {
//    return -1;
//  }
//
//  imageB_ = cam;  
//
//  return 0;
}


template <class InputStorageType, class OutputStorageType>
int BIAS::RectificationBase<InputStorageType, OutputStorageType>:: 
SetCameraB(const BIAS::Image<InputStorageType>& img,
           const BIAS::ProjectionParametersBase* proj) 
{
  if (!IsInputCameraValid(img, proj)) {
    return -1;
  }

  int res = 0;

  bool setProjection = false;
  if(ppBB_ == NULL) {
    setProjection = true;
  } else {
    setProjection = (ppBB_->DoExtrinsicsDiffer(proj) || ppBB_->DoIntrinsicsDiffer(proj));
  }

  if(setProjection)
  {
    delete ppBB_;
  ppBB_ = NULL;

  if (proj == NULL) {
    return -1;
  }

  ppBB_ = proj->Clone();
  } else {
    res = 1;
  }

  if (img.IsEmpty()) {
    return -1;
  }

  imageB_ = Camera<InputStorageType>(img);    

  return res;
}

template <class InputStorageType, class OutputStorageType>
void BIAS::RectificationBase<InputStorageType, OutputStorageType>:: 
GetRectifiedImageA(Image<OutputStorageType>& rectImg)
{
  rectImg = rectImageA_;
}

template <class InputStorageType, class OutputStorageType>
void BIAS::RectificationBase<InputStorageType, OutputStorageType>:: 
GetRectifiedImageB(Image<OutputStorageType>& rectImg)
{
  rectImg = rectImageB_;
}


template <class InputStorageType, class OutputStorageType>
Camera<InputStorageType>* BIAS::RectificationBase<InputStorageType, 
                                                  OutputStorageType>:: 
GetImagePointerA()
{
  return &imageA_;
}

template <class InputStorageType, class OutputStorageType>
Camera<InputStorageType>* BIAS::RectificationBase<InputStorageType, 
                                                  OutputStorageType>:: 
GetImagePointerB()
{
  return &imageB_;
}

template <class InputStorageType, class OutputStorageType>
const Image<OutputStorageType>* BIAS::RectificationBase<InputStorageType, 
                                                  OutputStorageType>:: 
GetRectifiedImagePointerA()
{
  return &rectImageA_;
}


template <class InputStorageType, class OutputStorageType>
const Image<OutputStorageType>* BIAS::RectificationBase<InputStorageType, 
                                                  OutputStorageType>:: 
GetRectifiedImagePointerB()
{
  return &rectImageB_;
}

//=========================== STATIC UTILS ===============================
template <class InputStorageType, class OutputStorageType>
int BIAS::RectificationBase<InputStorageType, OutputStorageType>::
CalculateMeanOrientation(const Pose& poseLeft,
                         const Pose& poseCenter,
                         const Pose& poseRight,
                         Quaternion<double>& orientation)
{
  Vector3<double> cL = poseLeft.GetC();
  Vector3<double>  cR = poseRight.GetC(); 
  Vector3<double> cC = poseCenter.GetC();
  if (Equal((cL-cR).NormL2(), 0.0)) {
    return -1;
  }
  if (Equal((cL-cC).NormL2(), 0.0)) {
    return -1;
  }
  if (Equal((cR-cC).NormL2(), 0.0)) {
    return -1;
  }

  //calculate mean baseline vector
  Vector3<double> baseline = (cC-cL)+(cR-cL);
  baseline.Normalize();
  //   baseline/=3.0;
  //   Vector3<double> baselineNormalized = baseline;
  //   baselineNormalized.
  
  //calcaulte mean optical axis
  Vector3<double> meanOA;
  //first determine the components orthogonal to the baseline
  Vector3<double> z[3];
  enum {left=0, center, right};
  z[left] = poseLeft.GetZAxis();
  z[right] = poseRight.GetZAxis();
  z[center] = poseCenter.GetZAxis();
  Vector3<double> baselineParallel[3]; //the zAxis part parallel to the baseline
  baselineParallel[left] = baseline.ScalarProduct(z[left])*baseline;
  baselineParallel[right] = baseline.ScalarProduct(z[right])*baseline;
  baselineParallel[center] = baseline.ScalarProduct(z[center])*baseline;
  
  Vector3<double> baselineOrtho[3]; //the zAxis part parallel to the baseline
  baselineOrtho[left] = z[left]-baselineParallel[left];  
  baselineOrtho[right] = z[right]-baselineParallel[right];
  baselineOrtho[center] = z[center]-baselineParallel[center];

  meanOA = baselineOrtho[left]+baselineOrtho[right]+baselineOrtho[center];
  meanOA.Normalize();
  cerr<<"ortho? = "<<meanOA.ScalarProduct(baseline)<<endl;
  Vector3<double> newYAxis = meanOA.CrossProduct(baseline);
  //  newYAxis.Normalize();//unneccessary !
  RMatrix resOrientation;
  for(unsigned int i=0; i<3; i++) {
    resOrientation[i][0] = baseline[i];
    resOrientation[i][1] = newYAxis[i];
    resOrientation[i][2] = meanOA[i];
  }
  cerr<<resOrientation.GetDeterminant()<<endl;
  cerr<<resOrientation<<endl;
  resOrientation.GetQuaternion(orientation);

  return 0;
}

template <class InputStorageType, class OutputStorageType>
int BIAS::RectificationBase<InputStorageType, OutputStorageType>::
CalculateRectifiedBases(const Pose& poseA,
                        const Pose& poseB,
                        Pose& poseResA,
                        Pose& poseResB,
                        bool failIfForwardMove)
{
  if (Equal((poseA.GetC() - poseB.GetC()).NormL2(), 0.0)) {
    BIASERR("poses for rectification have equal C" << endl)
    return -1;
  }

  Vector3<double> C[2], A[2]; // decomposed original cams
  RMatrix R1,R2;
  Vector3<double> Ap[2]; // decomposed virtual planar cams  
  Vector3<double> B; // Baseline vector  
  Vector3<double> Ac, Hc, Vc;

  R1 = poseA.GetR();
  C[0] = poseA.GetC();
  A[0].Set(R1[0][2], R1[1][2], R1[2][2]);

  R2 = poseB.GetR();
  C[1] = poseB.GetC();
  A[1].Set(R2[0][2], R2[1][2], R2[2][2]);
 
  B = C[1] - C[0];
  B.Normalize();
  
  double scalarProduct[2];

  for (unsigned int i=0;i<2;i++){    
    //Project A onto B. Compute vector in direction of B and appropiate length.
    //Redirect the imageplane normal A to Ap.
    // Resulting Ap is orthonormal to B.  

    scalarProduct[i] = A[i].ScalarProduct(B);
    Ap[i] = A[i] - B * (scalarProduct[i]);
    if(fabs(scalarProduct[i]) != 1.0)
      Ap[i].Normalize();      
    else { //one camera is viewing along the baseline => forward move
      if(failIfForwardMove){
        BIASERR("no cylindrical rectification in case of forwar move" << endl)
        return -2;
      }
    }
      
  }

  bool first = Equal(fabs(scalarProduct[0]), 1.0);
  bool second = Equal(fabs(scalarProduct[1]), 1.0); 
  
  if( first || second) {
    //this case takes place when one of two cameras is viewing along 
    //the baseline
    //multiplication by scalarProduct explained:
    //if both cameras are facing each other or are rolled against each other
    // then the following operation of taking the half vector can fail so
    // both rectifying vectors Ap should lie in the same halfspace
    //choose basevector perpendicular to viewing direction
    if(first) {
      Ap[0].Set(R1[0][0], R1[1][0], R1[2][0]);   
      Ap[0]*= -scalarProduct[0];
    }
    if(second) {
      Ap[1].Set(R2[0][0], R2[1][0], R2[2][0]);    
      Ap[1]*= -scalarProduct[1];
    }
  }
  
     //normal vector of plane lies inbetween redirected imageplane normals
  Ac = Ap[0] + Ap[1];
  if(Equal(Ac.NormL2(), 0.0)) {
    //if only one camera was viewing in direction of the baseline it might 
    // happen that the chosen direction of Ap is parallel to the other.
    // decide for the other so that at least one rect image is close to
    // viewing direction.
    if(second)
      Ac = Ap[0];
    if(first)
      Ac = Ap[1];
  }
  Ac.Normalize();
  Hc = B; 
  Vc = Ac.CrossProduct(Hc);  
  
  
  RMatrix resR;
  for(unsigned int i=0; i<3; i++) {
    resR[i][0] = Hc[i];
    resR[i][1] = Vc[i];
    resR[i][2] = Ac[i];
  }

  Quaternion<double> Q;
  resR.GetQuaternion(Q);
  poseResA.Set(Q, C[0]);
  poseResB.Set(Q, C[1]);  
  return 0;
}



namespace BIAS{
// TODO FIXME win32 template instantiation problem (JW)
template class RectificationBase<unsigned char, unsigned char>;
template class RectificationBase<float, float>;
template class RectificationBase<unsigned char, float>;
template class RectificationBase<float, unsigned char>;

#if defined(BUILD_IMAGE_CHAR)
// TODO FIXME win32 template instantiation problem (JW)
template class RectificationBase<unsigned char, char>;
template class RectificationBase<char, char>;
#endif

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

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

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

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

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

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

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