CalibratedPyramid.cpp

#include "CalibratedPyramid.hh"

#include <Base/Math/Vector2.hh>
#include <Geometry/ProjectionParametersBase.hh>
#include <Base/Image/ImageIO.hh>

using namespace BIAS;
using namespace std;

template <class StorageType>
CalibratedPyramid<StorageType>::
CalibratedPyramid()
  : PyramidImage_(), Data_(), PositionOffset_(0.)
{
  const int size = PyramidImage_.Size();
  Data_.resize(size);
  for (int i=0; i<size; i++){
    Data_[i].projection = SharedPtr<ProjectionParametersBase>(NULL);
    Data_[i].image = PyramidImage_[i];
  }
}


template <class StorageType>
CalibratedPyramid<StorageType>::
CalibratedPyramid(const CalibratedPyramid<StorageType>& cp)
  : PyramidImage_(), Data_(PyramidImage_.Size())
{
  operator=(cp);
}


template <class StorageType>
CalibratedPyramid<StorageType>::
CalibratedPyramid(const PyramidImage<StorageType>& cp)
  : PyramidImage_(cp), Data_(cp.size()), PositionOffset_(cp.GetPositionOffset())
{
  const unsigned size = Size();
  for (unsigned i=0; i<size; i++){
    Data_[i].projection = SharedPtr<ProjectionParametersBase>(NULL);
    Data_[i].image = PyramidImage_[i];
  }
}


template <class StorageType>
CalibratedPyramid<StorageType>::
~CalibratedPyramid()
{
  Clear();
}

template <class StorageType>
void CalibratedPyramid<StorageType>::
Init(const CalibratedImage<StorageType>& ci,
     const unsigned pyramid_size, const double rescale_factor)
{
  BIASASSERT(pyramid_size>0);
  Clear();
  PyramidImage_.SetRescaleFactor(rescale_factor);
  const unsigned ui_factor = (unsigned)rint(rescale_factor);
  
  if ( (ui_factor==2 || ui_factor==4) && 
       Equal((double)ui_factor, rescale_factor) && false){
    // for some mysterious reason, using the own downsampling routine gives
    // slightly worse results on VisionN/Stereo/Testst/TestTrifocalDisparity
    // with the Aloe test date, it is tus disabled
    PositionOffset_ = ((double)(ui_factor)*0.5)-0.5;
    PyramidImage_.clear();
    PyramidImage_.Init(*(ci.image), pyramid_size);
    PyramidImage_.resize(pyramid_size);
    cout << "CalibratedPyramid: using own downsampling\n";
    *PyramidImage_[0] = *ci.image;
    for (unsigned p=1; p<pyramid_size; p++){
      Downsample_(PyramidImage_[p-1], PyramidImage_[p], ui_factor);
    }
  } else { 
    cout << "CalibratedPyramid: using downsampling from Resize class\n";
    PyramidImage_.Init(*(ci.image), pyramid_size);
    PyramidImage_.Downsample();
    PositionOffset_ = PyramidImage_.GetPositionOffset();
  }
  cout << "  using position offset "<<PositionOffset_<<" for coo transfer\n";
  cout << "  using rescale factor: "<<rescale_factor<<" "
       <<PyramidImage_.GetRescaleFactor()<<endl;

  Data_.resize(pyramid_size);
  if (ci.projection)
    Data_[0].projection = 
      SharedPtr<ProjectionParametersBase>(ci.projection->Clone());
  else 
    Data_[0].projection = SharedPtr<ProjectionParametersBase>(NULL);
  Data_[0].image = PyramidImage_[0];
  for (unsigned i=1; i<pyramid_size; i++){
    if (Data_[i-1].projection){
      Data_[i].projection = 
        SharedPtr<ProjectionParametersBase>(Data_[i-1].projection->Clone());
      Data_[i].projection->Rescale(rescale_factor, PositionOffset_);
      //Data_[i].projection->Rescale(rescale_factor);
    } else {
      Data_[i].projection = SharedPtr<ProjectionParametersBase>(NULL);
    }
    Data_[i].image = PyramidImage_[i];
  }
}


template <class StorageType>
void CalibratedPyramid<StorageType>::
Clear()
{
  Data_.clear();
}

template <class StorageType>
CalibratedPyramid<StorageType>& CalibratedPyramid<StorageType>::
operator=(const CalibratedPyramid<StorageType>& cp)
{
  Clear();
  const int size = cp.Size();
  PyramidImage_ = cp.PyramidImage_;
  Data_.resize(size);
  for (int i=0; i<size; i++){
    if (cp[i].projection)
      Data_[i].projection = SharedPtr<ProjectionParametersBase>(cp[i].projection->Clone());
    if (cp[i].image)
      Data_[i].image = PyramidImage_[i];
  }
  PositionOffset_ = cp.PositionOffset_;

  return *this;
}


template <class StorageType>
void CalibratedPyramid<StorageType>::
CoordinateTransf(const Vector2<double>& coo0, const unsigned index,
                 Vector2<double>& coo_index) const
{
  if (index==0) { coo_index=coo0; return ; }
  coo_index[0] = (coo0[0]-PositionOffset_) / 
    (PyramidImage_.GetFactors()[index]);
  coo_index[1] = (coo0[1]-PositionOffset_) / 
    (PyramidImage_.GetFactors()[index]);
  //  coo_index[0] = coo0[0] / PyramidImage_.GetFactors()[index];
  //coo_index[1] = coo0[1] / PyramidImage_.GetFactors()[index];
}


template <class StorageType>
void CalibratedPyramid<StorageType>::
Downsample_(SharedPtr<const Image<StorageType> > src, 
            SharedPtr<Image<StorageType> > dst, const unsigned factor) const
{
  BIASASSERT((factor == 2) | (factor == 4));
  Binomial<StorageType, StorageType> bin;
  bin.SetBorderHandling(FilterBase<StorageType, StorageType>::TBH_same);
  bin.SetHalfWinSize(factor/2); // 1 or 2, i.e. 3x3 or 5x5
  Image<StorageType> filtered;
  bin.Filter(*src, filtered);
  BIASASSERT(filtered.GetWidth() == src->GetWidth() && 
             filtered.GetHeight() == src->GetHeight() && 
             filtered.GetChannelCount() == src->GetChannelCount());
  const int ow = src->GetWidth(), oh = src->GetHeight();
  const int cc = src->GetChannelCount();
  const int nw = ow / factor, nh = oh / factor;
  if ((int)dst->GetWidth() != nw || (int)dst->GetHeight() != nh ||
      (int)dst->GetChannelCount() != cc ) {
    if (!dst->IsEmpty()){
      dst->Release();
    }
    dst->Init(nw, nh, cc);
  }
  if (!dst->GetColorModel() != src->GetColorModel()){
    dst->SetColorModel(src->GetColorModel());
  }

  StorageType **dst_ida = dst->GetImageDataArray();
  StorageType **fil_ida = filtered.GetImageDataArray();
  int x, y, c, xc, yb, xb, xbc;
#ifdef _OPENMP
# pragma omp parallel for private (x, y, c, xc, yb, xb, xbc)
#endif
  for (y=0; y<nh; y++){
    yb = y*factor + (int)PositionOffset_;
    for (x=0; x<nw; x++){
      xb = x * factor + (int)PositionOffset_;
      xbc = xb * cc;
      xc = x * cc;
      for (c=0; c<cc; c++){
        dst_ida[y][xc+c] = fil_ida[yb][xbc+c];
      }
    }
  }
}
        

//////////////////////////////////////////////////////////////////////////
//                 instantiation
//////////////////////////////////////////////////////////////////////////

#define INST(type) template class BIASImage_EXPORT CalibratedPyramid<type>
namespace BIAS{
INST(unsigned char);
INST(float);

#ifdef BUILD_IMAGE_INT
INST(int);
#endif
#ifdef BUILD_IMAGE_CHAR
INST(char);
#endif
#ifdef BUILD_IMAGE_SHORT
INST(short);
#endif
#ifdef BUILD_IMAGE_USHORT
INST(unsigned short);
#endif
#ifdef BUILD_IMAGE_UINT
INST(unsigned int);
#endif
#ifdef BUILD_IMAGE_DOUBLE
INST(double);
#endif
}