ProjectionMapping.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/ProjectionMapping.hh"
#include <Geometry/RMatrix.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Geometry/Projection.hh>
#include <Base/Math/Random.hh>
#include <list>

using namespace BIAS;
using namespace std;


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

template <class InputStorageType, class OutputStorageType>
ProjectionMapping<InputStorageType, OutputStorageType>::
ProjectionMapping()
: SourceP_(ProjectionParametersPerspective()), 
  SinkP_(ProjectionParametersPerspective()),
  depthMapSink_(NULL), mappingPlane_(0,0,0,1),
  accountForDifferentCameraCenters_(false)
{
  relativeRSinkToSource_.SetIdentity();
}


template <class InputStorageType, class OutputStorageType>
ProjectionMapping<InputStorageType, OutputStorageType>::
ProjectionMapping(const ProjectionMapping & src) : 
  depthMapSink_(NULL),
  accountForDifferentCameraCenters_(false)
{
  (*this) = src;
}

template <class InputStorageType, class OutputStorageType>
ProjectionMapping<InputStorageType, OutputStorageType> & 
ProjectionMapping<InputStorageType, OutputStorageType>::
operator=(const ProjectionMapping & src)
{
   SourceP_ = src.SourceP_;
   SinkP_ = src.SinkP_;
   accountForDifferentCameraCenters_  = src.accountForDifferentCameraCenters_;
   depthMapSink_ = src.depthMapSink_;
   mappingPlane_ = src.mappingPlane_;
   return *this;
}

template <class InputStorageType, class OutputStorageType>
int ProjectionMapping<InputStorageType, OutputStorageType>::
GetBoundingBox(unsigned int srcwidth,
           unsigned int srcheight,
           unsigned int sinkwidth,
           unsigned int sinkheight,
           int &tlx, int &tly,
           int &brx, int &bry){


  // do a forward mapping of the corners of the source image and compute
  // bounding box, where we need to operate in sink.
  // This is used to speed up stitching of huge mosaics
  // where each source image spans just a tiny area of the final mosaic.
  // I know there might be unsupported cases with wrap-aorund effects
  // (e.g. omnidirectional image with more than 180° warped into another
  // ultra wide angle image facing to the opposite side) where some
  // parts of the sink are empty although they should not.
  // If you want to fix the code for this case, look at the ordering
  // of the warped points rather than just the bounding box
  // and adapt this function. Kevin

  std::list<HomgPoint2D> borderpoints;
  borderpoints.push_back(HomgPoint2D(0,0,1));
  borderpoints.push_back(HomgPoint2D(srcwidth/2,0,1));
  borderpoints.push_back(HomgPoint2D(srcwidth-1,0,1));

  borderpoints.push_back(HomgPoint2D(0,srcheight/2,1));
  borderpoints.push_back(HomgPoint2D(srcwidth/2,srcheight/2,1));
  borderpoints.push_back(HomgPoint2D(srcwidth-1,srcheight/2,1));

  borderpoints.push_back(HomgPoint2D(0,srcheight-1,1));
  borderpoints.push_back(HomgPoint2D(srcwidth/2,srcheight-1,1));
  borderpoints.push_back(HomgPoint2D(srcwidth-1,srcheight-1,1));

  double dtlx=sinkwidth, dtly=sinkheight, dbrx=-1.0, dbry=-1.0;

  // quick hack: swap source and sink projection
  Projection Ptemp = SourceP_;
  SourceP_ = SinkP_;
  SinkP_ = Ptemp;

  int countsuccessfulsamples = 0;
  list<HomgPoint2D>::iterator bit = borderpoints.begin();
  for (; bit!=borderpoints.end(); bit++) {
    HomgPoint2D sinkpoint;
    int mapresult =this->GetSourceCoordinates_(*bit,sinkpoint);
    if (mapresult<0) {
      BIASWARN(*bit<<" is mapped to "<<sinkpoint<<" with an error: "<<mapresult);
      continue; // ignore this point for bounding box approximation
    }
    //   cout<< *bit <<" (src) mapped to "<<sinkpoint<<endl;
    if (sinkpoint[0]<dtlx) dtlx = sinkpoint[0];
    if (sinkpoint[1]<dtly) dtly = sinkpoint[1];
    if (sinkpoint[0]>dbrx) dbrx = sinkpoint[0];
    if (sinkpoint[1]>dbry) dbry = sinkpoint[1];
    countsuccessfulsamples++;
  }
  SinkP_ = SourceP_;
  SourceP_ = Ptemp;

  if (countsuccessfulsamples<7) {
    // we might have missed an entire boundary, lets be conservative and map whole image
    tlx = 0;
    tly = 0;
    brx = sinkwidth-1;
    bry = sinkheight-1;
    BIASWARNONCE("approximation of bounding box in sink failed with only "<<countsuccessfulsamples
                 <<" of 9 samples. Using whole image. Might be slow.");
    return -1;
  }

  // check upper integer limits
  if (dtlx > sinkwidth-1) dtlx = sinkwidth-1;
  if (dtly > sinkheight-1) dtly = sinkheight-1;
  if (dbrx > sinkwidth) dbrx = sinkwidth;
  if (dbry > sinkheight) dbry = sinkheight;
  // check lower integer limits
  if (dtlx < -1.0) dtlx = -1.0;
  if (dtly < -1.0) dtly = -1.0;
  if (dbrx < 0.0) dbrx = 0.0;
  if (dbry < 0.0) dbry = 0.0;

  tlx = ( int)dtlx-1;
  tly = ( int)dtly-1;
  brx = ( int)ceil(dbrx)+1;
  bry = ( int)ceil(dbry)+1;

  return 0;

}


template <class InputStorageType, class OutputStorageType>
int ProjectionMapping<InputStorageType, OutputStorageType>::
GetSourceCoordinates_(const HomgPoint2D & sink,
                      HomgPoint2D & source) const {
  // use scene information or only rotation ?
  if (accountForDifferentCameraCenters_) { 
    // the reconstructed point on the current ray in global coordinates
    HomgPoint3D pglobal;
    // depth map or map via plane ?
    if (depthMapSink_==NULL) {
      // construct 3D points on mappingPlane_
      // get ray dir
      Vector3<double> p, d;
      SinkP_.GetParameters()->UnProjectToRay(sink, p, d);
      HomgPoint3D Ray(d);
      // make ray from homgpoint
      Ray[3] = 0;
      if (Ray.NormL2()<0.5) return -1;
      // intersect with plane, if parallel, use point at infinity
      if (mappingPlane_.
          GetLineIntersection(HomgPoint3D(SinkP_.GetParameters()->GetC()),
                              Ray, pglobal)<0) return -1;
    } else {
      // construct 3D point in global coordinates using depth map
      const double depth = 
        depthMapSink_->GetImageDataArray()[int(sink[1])][int(sink[0])];
      if (depth<=0.0) return -1; // no depth information

      pglobal = SinkP_.GetParameters()->UnProjectToPoint(sink, depth);
      if (pglobal[3]==0.0) return -1;
    }
    // project using global pose and internals:
    source = SourceP_.GetParameters()->Project(pglobal);
  } else {
    // ### DEFAULT CASE WITHOUT 3D ###
    // compute ray in camera coordinate system,rotate ray,apply new intrinsics
    // ideal perspective: x_src = K_src * R * K_sink^-1 * x_sink 
    Vector3<double> p, d;
    SinkP_.GetParameters()->UnProjectLocal(sink, p, d);
    source = SourceP_.GetParameters()->
      ProjectLocal(relativeRSinkToSource_ * d);

    unsigned int width, height;
    SourceP_.GetParameters()->GetImageSize(width,height);
    if(source[0] < 0 || source[0] >= width || 
       source[1] < 0 || source[1] >= height )
      return 1;
  }

  // check if source (x,y,0) or (0,0,0) and if so return -1 
  if (source[2]< 0.5) {
    return 1;
  }
  return 0;
}

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

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

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

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

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

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

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

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

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