ContourDetectorBSpline.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 <fstream>

#include <Base/Debug/Debug.hh>
#include <Base/Debug/Error.hh>
#include <Base/ImageUtils/ImageDraw.hh>
#include <Base/Math/Matrix2x2.hh>
#include <Filter/GradientSobel3x3.hh>
#include <MathAlgo/SVD.hh>


#include "ContourDetectorBSpline.hh"

using namespace BIAS;
using namespace std;

template <class StorageType>
ContourDetectorBSpline<StorageType>::
ContourDetectorBSpline(){
  pData_=NULL;
  pShapeSpaceMatrix_=NULL;
  pSubShapeSpaceMatrix_=NULL;
  pRegMatrix_=NULL;
  curveInitialised_=false;
  fitSampleWidth_= new double(0.1);
  drawSampleWidth_= new double(0.1);
}


template <class StorageType>
ContourDetectorBSpline<StorageType>::
~ContourDetectorBSpline(){
  if(pData_!=NULL){pData_->Unregister();}
  if(pShapeSpaceMatrix_!=NULL){pShapeSpaceMatrix_->Unregister();}
  if(pSubShapeSpaceMatrix_!=NULL){pSubShapeSpaceMatrix_->Unregister();}
  if(pRegMatrix_!=NULL){pRegMatrix_->Unregister();}
}


template <class StorageType>
int ContourDetectorBSpline<StorageType>::
Detect(Image<StorageType>& image,
       std::vector<BIAS::BIASContour>& contour){

  BIASWARN("Unfinished!");
  return -1;
}


//public:
template <class StorageType> ContourDetectorBSpline<StorageType>
ContourDetectorBSpline<StorageType>::
Cluster(const std::vector<ContourDetectorBSpline<StorageType> >& bSplines){

  unsigned int i,j; //counter variables
  //build new object
  ContourDetectorBSpline<StorageType> bSpline;

  //register with data objects from given B-Spline curves
  std::vector<ContourBSplineData*> bSplineData;
  for(i=0;i<bSplines.size();i++){
    bSplineData.push_back(bSplines[i].pData_);
  }
  bSpline.pData_= ContourBSplineData::Register(bSplineData);

  //build new Q
  unsigned int numBasePolynoms= (bSpline.pData_->numBasePolynoms_);//new
  unsigned int splinesNumBasePolynoms;//from particular curves
  unsigned int pos=0;
  BIAS::Vector<double> Q(2*numBasePolynoms);
  for(i=0;i<bSplines.size();i++){
    splinesNumBasePolynoms=((bSplines[i]).pData_)->numBasePolynoms_;
    for(j=0;j<splinesNumBasePolynoms;j++){
      Q[pos+j]=((bSplines[i]).Q_)[j];
      Q[pos+j+numBasePolynoms]=((bSplines[i]).Q_)[j+splinesNumBasePolynoms];
    }
    pos+=splinesNumBasePolynoms;
  }
  bSpline.Q_=Q;
  return bSpline;
}

///////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
Init(const unsigned int order,
     const ContourBSplineType::Type bType,
     const std::vector<unsigned int>& mPnts,
     const std::vector<BIAS::Vector2<double> >& cPnts){
  BIAS::Vector<double> Q;

  //convert vector of control points into internal used format
  CPntsToQ_(cPnts,Q);
  Init(order, bType, mPnts, Q);
}

///////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
Init(const unsigned int order,
     const ContourBSplineType::Type bType,
     const std::vector<BIAS::Vector2<double> >& cPnts){

  //multiplicity for all control points is 1
  std::vector<unsigned int> mPnts(cPnts.size(),1);
  BIAS::Vector<double> Q;

  //convert vector of control points into internal used format
  CPntsToQ_(cPnts,Q);
  Init(order, bType, mPnts, Q);
}

///////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
Init(const unsigned int order,
     const ContourBSplineType::Type bType,
     const std::vector<unsigned int>& mPnts,
     const BIAS::Vector<double>& Q){
#ifdef BIAS_DEBUG
  TestInit_(order, bType, mPnts, Q);
#endif
  Q_=Q;
  cout<<"test init done"<<endl;
  //register data
  pData_=ContourBSplineData::Register(order, bType, mPnts);
  cout<<"register done"<<endl;
  BIASCDOUT(D_CONTOURBSPLINE_INIT,"metricmatrix: " <<
            pData_->splineMetricMatrix_ <<
            std::endl<<std::flush);
}

///////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
Init(const unsigned int order,
     const ContourBSplineType::Type bType,
     const BIAS::Vector<double>& Q){
  //multiplicity for all control points is 1
  std::vector<unsigned int> mPnts(Q.Size()/2,1);

  Init(order, bType, mPnts, Q);
}
///////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
InitCurve_(){
  unsigned int i; //counter variable
  unsigned int numBasePolynoms = pData_->numBasePolynoms_;
  unsigned int order = pData_->order_;
  unsigned int numSpans = pData_->numSpans_;

  // split control point vector into Qx and Qy
  BIAS::Matrix<double> Qx(numBasePolynoms,1);
  BIAS::Matrix<double> Qy(numBasePolynoms,1);
  for(i=0;i<numBasePolynoms;i++){
    Qx[i][0]=Q_[i];
    Qy[i][0]=Q_[i+numBasePolynoms];
  }

  curveVectors_.clear();

  //using equation on p.292 ("active contours") to compute coeff vectors
  std::vector<BIAS::Vector<double> > tmpBin(2);
  BIAS::Vector<double> tmpVecX(order);
  BIAS::Vector<double> tmpVecY(order);
  BIAS::Matrix<double> tmpMat(numBasePolynoms,1);
  for(i=0;i<numSpans;i++){
    //x part
    tmpMat=Qx;
    tmpMat.MultLeft((pData_->placedSpanMatrices_)[i]);
    tmpVecX=tmpMat.GetCol(0);
    //y part
    tmpMat=Qy;
    tmpMat.MultLeft((pData_->placedSpanMatrices_)[i]);
    tmpVecY=tmpMat.GetCol(0);

    tmpBin[0]=tmpVecX;
    tmpBin[1]=tmpVecY;
    curveVectors_.push_back(tmpBin);
  }
  curveInitialised_=true;
}
///////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
Fit(BIAS::Image<StorageType>& greyImg,
    const double& alpha, const unsigned int normalWidth){
  //possibly init curve vectors
  if(!curveInitialised_){ InitCurve_(); }

  //build gradient image with ROI
  BIAS::Image<float> featImg;
  GetFeatImage_(greyImg, featImg, normalWidth);

  Fit_(featImg, alpha, normalWidth);
}

///////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
Fit(BIAS::Image<StorageType>& greyImg,
    const double& alpha, const unsigned int normalWidth,
    const double& minClip){
  //possibly init curve vectors
  if(!curveInitialised_){ InitCurve_(); }

  //build gradient image with ROI
  BIAS::Image<float> featImg;
  GetFeatImage_(greyImg, featImg, normalWidth);

  Fit_(featImg, alpha, normalWidth, minClip);
}

///////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
Fit(const BIAS::Image<float>& featImg,
    const double& alpha, const unsigned int normalWidth){
  //possibly init curve vectors
  if(!curveInitialised_){ InitCurve_(); }
  Fit_(featImg, alpha, normalWidth);
}

///////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
Fit(const BIAS::Image<float>& featImg,
    const double& alpha, const unsigned int normalWidth,
    const double& minClip){
  if(!curveInitialised_){ InitCurve_(); }
  Fit_(featImg, alpha, normalWidth, minClip);
}

///////////////////////////////////////////////////
template <class StorageType> bool
ContourDetectorBSpline<StorageType>::
Save(const std::string& filename){
  unsigned int i,j,k; //counter variables

  std::ofstream fs;
  fs.open(filename.c_str(), std::ofstream::out | std::ofstream::binary);
  if(!fs.is_open())return false;

  //store order
  fs.write( (char*) &(pData_->order_), sizeof(unsigned int) );

  //store BSpline type
  fs.write( (char*) &(pData_->bType_), sizeof(ContourBSplineType::Type) );

  //store size of multiplicity and controlpoint vector (=numBasePolynoms)
  fs.write( (char*) &(pData_->numBasePolynoms_), sizeof(unsigned int) );

  //store multiplicity vector
  for(i=0;i<(pData_->numBasePolynoms_);i++){
    fs.write( (char*) &((pData_->mPnts_)[i]), sizeof(unsigned int) );
  }

  //store controlpoint vector (plain)
  for(i=0;i<2*(pData_->numBasePolynoms_);i++){
    fs.write( (char*) &(Q_[i]), sizeof(double) );
  }

  //for clustered curve there is more to save (numSpans & placedSpanMatrices)
  if(pData_->bType_==ContourBSplineType::Cluster){
    //store numSpans
    fs.write( (char*) &(pData_->numSpans_), sizeof(unsigned int) );

    //store placedSpanMatrices
    for(i=0;i<pData_->numSpans_;i++){
      for(j=0;j<pData_->order_;j++){
        for(k=0;k<pData_->numBasePolynoms_;k++){
          fs.write( (char*) &((pData_->placedSpanMatrices_)[i][j][k]),
                    sizeof(double) );
        }
      }
    }
    //store numSpansVec size
    unsigned int numSpansVecSize=(pData_->numSpansVec_).size();
    fs.write( (char*) &numSpansVecSize, sizeof(unsigned int) );

    //store numSpansVec
    for(i=0;i<numSpansVecSize;i++){
      fs.write( (char*) &((pData_->numSpansVec_)[i]), sizeof(unsigned int) );
    }
  }

  //close filestream
  fs.close();
  return true;
}

///////////////////////////////////////////////////
template <class StorageType> bool
ContourDetectorBSpline<StorageType>::
Load(const std::string& filename){
  unsigned int i,j,k; //counter variables

  std::ifstream fs;
  fs.open(filename.c_str(), std::ifstream::in | std::ifstream::binary);
  if(!fs.is_open()){
    return false;
  }
  //load order
  unsigned int order;
  fs.read( (char*) &order, sizeof(unsigned int) );

  //load BSpline type
  ContourBSplineType::Type bType;
  fs.read( (char*) &bType, sizeof(ContourBSplineType::Type) );

  //load num of base polynoms
  unsigned int numBasePolynoms;
  fs.read( (char*) &numBasePolynoms, sizeof(unsigned int) );

  //load multiplicity vector
  std::vector<unsigned int> mPnts(numBasePolynoms);
  for(i=0; i<numBasePolynoms; i++){
    fs.read( (char*) &mPnts[i], sizeof(unsigned int) );
  }

  //load controlpoint vector (plain)
  BIAS::Vector<double> Q(2*numBasePolynoms);
  for(i=0; i<2*numBasePolynoms; i++){
    fs.read( (char*) &Q[i], sizeof(double) );
  }

  //if we have a Cluster BSpline ... there is more to load
  if(bType==ContourBSplineType::Cluster){
    //load numSpans
    unsigned int numSpans;

    fs.read( (char*) &numSpans, sizeof(unsigned int) );

    //load placedSpanMatrices
    std::vector<BIAS::Matrix<double> > placedSpanMatrices;
    BIAS::Matrix<double> placedSpanMatrix(order,numBasePolynoms);
    for(i=0;i<numSpans;i++){
      for(j=0;j<order;j++){
        for(k=0;k<numBasePolynoms;k++){
          fs.read( (char*) &(placedSpanMatrix[j][k]), sizeof(double) );
        }
      }
      placedSpanMatrices.push_back(placedSpanMatrix);
    }

    //load numSpansVec size
    unsigned int numSpansVecSize;
    fs.read( (char*) &numSpansVecSize, sizeof(unsigned int) );

    //load numSpansVec
    std::vector<unsigned int> numSpansVec(numSpansVecSize);
    for(i=0;i<numSpansVecSize;i++){
      fs.read( (char*) &(numSpansVec[i]), sizeof(unsigned int) );
    }

    //build up bSpline
    Q_=Q;
    pData_=ContourBSplineData::Register(order, bType, mPnts, numBasePolynoms,
                                        numSpans, numSpansVec,
                                        placedSpanMatrices);

  }else{
    //build up bSpline (normal case)
    Init(order, bType, mPnts, Q);
  }

  //close filestream
  fs.close();

  return true;
}
/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
Rotate(const double& angle){
  //curve must have its center at zero
  unsigned int i; //counter variable

  BIAS::Vector2<double> tmp,tmp2;
  BIAS::Matrix2x2<double> rot;//rotation matrix
  rot[0][0]=cos(angle);
  rot[0][1]=-sin(angle);
  rot[1][0]=sin(angle);
  rot[1][1]=cos(angle);
  for(i=0;i<pData_->numBasePolynoms_;i++){
    tmp[0]=Q_[i];
    tmp[1]=Q_[i+(pData_->numBasePolynoms_)];
    rot.Mult(tmp,tmp2);
    Q_[i]=tmp2[0];
    Q_[i+(pData_->numBasePolynoms_)]=tmp2[1];
  }
}

template <class StorageType> void
ContourDetectorBSpline<StorageType>::
GetPoint(const double& t, BIAS::Vector2<double>& res){
  if(!curveInitialised_){ InitCurve_(); }
  unsigned int span;
  unsigned int numSpans= pData_->numSpans_;
  double s;
  BIAS::Vector<double> sVec(pData_->order_); //curve parametric vector

  //compute which span to use and convert t regarding span
  if(t<=0.){
    span=0;
    s=0.;
  }
  else if(t>=1.){
    span=numSpans-1;
    s=1.;
  }
  else{
    span = (unsigned int) (t * (double)(numSpans));
    s= (t*numSpans) - span;
  }

  //finaly get point
  GetPointAndParamVec_(span,s,sVec,res);
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
GetNormal(const double& t, BIAS::Vector2<double>& res){
  if(!curveInitialised_){ InitCurve_();}

  unsigned int span;
  unsigned int numSpans=pData_->numSpans_;
  double s;

  //compute which span to use and convert t regarding span
  if(t<=0.){
    span=0;
    s=0.;
  }
  else if(t>=1.){
    span=numSpans-1;
    s=1.;
  }
  else{
    span = (unsigned int) (t * (double)(numSpans) );
    s= (t*numSpans) - span;
  }

  //finaly get normal
  GetNormal_(span,s,res);
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
GetFeature(BIAS::Image<StorageType>& greyImg,
           const double& t,
           const unsigned int normalWidth,
           BIAS::Vector2<double>& res){
  //convert input image into gradient image with ROI
  BIAS::Image<float> featImg;
  GetFeatImage_(greyImg,featImg,normalWidth);

  //finally get feature point
  GetFeature(featImg, t, normalWidth, res);
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
GetFeature(BIAS::Image<StorageType>& greyImg,
                                const double& t,
                                const unsigned int normalWidth,
                                const double& minClip,
                                BIAS::Vector2<double>& res){
  //convert input image into gradient image
  BIAS::Image<float> featImg;
  GetFeatImage_(greyImg,featImg,normalWidth);

  //finally get feature point
  GetFeature(featImg, t, normalWidth, minClip, res);
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
GetFeature(const BIAS::Image<float>& featImg,
                                const double& t,
                                const unsigned int normalWidth,
                                BIAS::Vector2<double>& res){
  //possibly initialise curve vectors
  if(!curveInitialised_){ InitCurve_(); }

  unsigned int span; //current span
  unsigned int numSpans=pData_->numSpans_;
  double s;

  //compute which span to use and convert t regarding span
  if(t<=0.){
    span=0;
    s=0.;
  }
  else if(t>=1.){
    span=numSpans-1;
    s=1.;
  }
  else{
    span = (unsigned int) (t * (double)(numSpans) );
    s= (t*numSpans) - span;
  }

  BIAS::Vector2<double> curvePnt;
  BIAS::Vector2<double> normal;
  //compute curve point at t
  GetPoint_(span,s,curvePnt);
  //compute normal at t
  GetNormal_(span,s,normal);
  //finally get feature point
  GetFeature(featImg, normalWidth, curvePnt, normal, res);
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
GetFeature(const BIAS::Image<float>& featImg,
           const double& t,
           const unsigned int normalWidth,
           const double& minClip,
           BIAS::Vector2<double>& res){
  //possibly initialise curve vectors
  if(!curveInitialised_){ InitCurve_(); }

  unsigned int span;
  unsigned int numSpans=pData_->numSpans_;
  double s;

  //compute which span to use and convert t regarding span
  if(t<=0.){
    span=0;
    s=0.;
  }
  else if(t>=1.){
    span=numSpans-1;
    s=1.;
  }
  else{
    span = (unsigned int) (t * (double)(numSpans) );
    s= (t*numSpans) - span;
  }
  BIAS::Vector2<double> curvePnt;
  BIAS::Vector2<double> normal;
  //compute curve point at t
  GetPoint_(span,s,curvePnt);
  //compute normal at t
  GetNormal_(span,s,normal);
  //finally get feature point at t
  GetFeature(featImg, normalWidth, curvePnt, normal, minClip, res);
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
GetBoundingBox(int& minX, int& minY,
               int& maxX, int& maxY){
  unsigned int i; //counter variable
  unsigned int offset = Q_.size()/2;
  int tmpX, tmpY;

  minX=(int) (Q_[0]+0.5);
  maxX=(int) (Q_[0]+0.5);
  minY=(int) (Q_[offset]+0.5);
  maxY=(int) (Q_[offset]+0.5);

  for(i=1;i<offset;i++){
    tmpX=(int) (Q_[i]+0.5);
    tmpY=(int) (Q_[offset+i]+0.5);
    if(tmpX < minX){
      minX=tmpX;
    }
    else if(tmpX > maxX){
      maxX=tmpX;
    }
    if(tmpY < minY){
      minY=tmpY;
    }
    else if(tmpY > maxY){
      maxY=tmpY;
    }
  }
  BIASCDOUT(D_CONTOURBSPLINE_BOUNDINGBOX, "BSplines Bounding Box is: (" <<
            minX << "," << minY << ") (" << maxX << "," << maxY <<")." << std::endl <<
            std::flush);
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
DrawControlPoints(BIAS::Image<StorageType>& img,
                  const StorageType color[3]){
  unsigned int i; //counter variable


  for(i=0; i<pData_->numBasePolynoms_;i++){
    BIAS::ImageDraw<StorageType>::CircleCenterFilled(img,
                                                       (unsigned int)Q_[i],
                                                       (unsigned int)Q_[i+pData_->numBasePolynoms_],
                                                       2, color);
  }
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
DrawCurve(BIAS::Image<StorageType> &img,
                               const StorageType color[3]){
  if(!curveInitialised_){ InitCurve_(); }

  unsigned int i,j; //counter variable
  double t; //counter variable

  BIAS::Vector2<double> first;
  BIAS::Vector2<double> second;

  //convert sampleOffset_ relative to span
  double d = *drawSampleWidth_ * (double)pData_->numSpans_;
  unsigned int offset=0;

  for(i=0;i<(pData_->numSpansVec_).size();i++){
    GetPoint_(offset,0.,first);
    for(j=offset;j<((pData_->numSpansVec_)[i]+offset);j++){
      for(t=0.;t<(1.+d);t+=d){
        (t>1.)? GetPoint_(j,1,second):GetPoint_(j,t,second);
        BIAS::ImageDraw<StorageType>::Line(img,
                                           (unsigned int)first[0],
                                           (unsigned int)first[1],
                                           (unsigned int)second[0],
                                           (unsigned int)second[1],
                                           color);
        first=second;
      }
    }
    offset+=(pData_->numSpansVec_)[i];
  }
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
DrawCurve(BIAS::Image<float> &img){
  if(!curveInitialised_){ InitCurve_(); }

  unsigned int i,j; //counter variable
  double t; //counter variable
  float color[1]={255.};

  BIAS::Vector2<double> first;
  BIAS::Vector2<double> second;

  //convert sampleOffset_ relative to span
  double d = *drawSampleWidth_ * (double)pData_->numSpans_;
  unsigned int offset=0;

  for(i=0;i<(pData_->numSpansVec_).size();i++){
    GetPoint_(offset,0.,first);
    for(j=offset;j<((pData_->numSpansVec_)[i]+offset);j++){
      for(t=0.;t<(1.+d);t+=d){
        (t>1.)? GetPoint_(j,1,second):GetPoint_(j,t,second);
        BIAS::ImageDraw<float>::Line(img,
                                     (unsigned int)first[0],
                                     (unsigned int)first[1],
                                     (unsigned int)second[0],
                                     (unsigned int)second[1],
                                     color);
        first=second;
      }
    }
    offset+=(pData_->numSpansVec_)[i];
  }
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
DrawNormals(const unsigned int normalWidth,
            BIAS::Image<StorageType> &img,
            const StorageType color[3]){
  if(!curveInitialised_){ InitCurve_(); }
  unsigned int i;//counter variable
  double t; //counter variable

  unsigned int numSpans=pData_->numSpans_;

  BIAS::Vector2<double> curvePnt;
  BIAS::Vector2<double> normal;

  //convert sampleOffset_ relative to span
  double d = *fitSampleWidth_ * (double) numSpans;

  for(i=0;i<numSpans;i++){
    for(t=0.;t<1.+d;t+=d){
      if(t>1.){
        GetPoint_(i,1.,curvePnt);
        GetNormal_(i,1.,normal);
      }else{
        GetPoint_(i,t,curvePnt);
        GetNormal_(i,t,normal);
      }
      BIAS::ImageDraw<StorageType>::Line(img,
                                           (unsigned int)curvePnt[0],
                                           (unsigned int)curvePnt[1],
                                           (unsigned int)(curvePnt[0]+
                                                          (double)normalWidth*normal[0]),
                                           (unsigned int)(curvePnt[1]+
                                                          (double)normalWidth*normal[1]),
                                           color);

      BIAS::ImageDraw<StorageType>::Line(img,
                                           (unsigned int) curvePnt[0],
                                           (unsigned int) curvePnt[1],
                                           (unsigned int)(curvePnt[0]-
                                                          (double)normalWidth*normal[0]),
                                           (unsigned int)(curvePnt[1]-
                                                          (double)normalWidth*normal[1]),
                                           color);
    }
  }
}

//protected:

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
CPntsToQ_(const std::vector<BIAS::Vector2<double> >& cPnts){
  unsigned int i; //counter variable
  Q_.newsize(2*cPnts.size());

  for(i=0;i<cPnts.size();i++){
    Q_[i]=cPnts[i][0];
    Q_[i+cPnts.size()]=cPnts[i][1];
  }
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
CPntsToQ_(const std::vector<BIAS::Vector2<double> >& cPnts,
          BIAS::Vector<double>& Q){
  unsigned int i; //counter variable
  Q.newsize(2*cPnts.size());

  for(i=0;i<cPnts.size();i++){
    Q[i]=cPnts[i][0];
    Q[i+cPnts.size()]=cPnts[i][1];
  }
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
QToCPnts_(std::vector<BIAS::Vector2<double> >& cPnts){
  unsigned int i; //counter variable
  cPnts.resize(Q_.size()/2);
  for(i=0;i<cPnts.size();i++){
    cPnts[i][0]=Q_[i];
    cPnts[i][1]=Q_[i+cPnts.size()];
  }
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
QToCPnts_(const BIAS::Vector<double>& Q,
          std::vector<BIAS::Vector2<double> >& cPnts){
  unsigned int i; //counter variable
  cPnts.resize(Q.size()/2);
  for(i=0;i<cPnts.size();i++){
    cPnts[i][0]=Q[i];
    cPnts[i][1]=Q[i+cPnts.size()];
  }
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
GetPointAndParamVec_(const unsigned int span,
                     const double& s,
                     BIAS::Vector<double>& resParamVec,
                     BIAS::Vector2<double>& resPoint){
  unsigned int i,j; //loop variables

  unsigned int order=pData_->order_;

  //build sVec (parametric vector for computation of U matrix)
  resParamVec.newsize(order);
  resParamVec[0] = 1.;
  resParamVec[1] = s;
  resParamVec[2] = s*s;
  double tmpS;
  for(i=3;i<order;i++){ //only go through loops if order>3
    tmpS=s;
    for(j=0;j<(i-1);j++){
      tmpS*=s;
    }
    resParamVec[i]=tmpS;
  }
  //compute result point
  resPoint[0]=resParamVec.ScalarProduct(curveVectors_[span][0]);
  resPoint[1]=resParamVec.ScalarProduct(curveVectors_[span][1]);
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
GetNormal_(const unsigned int span, const double& s,
           BIAS::Vector2<double>& res){
  unsigned int i,j; //loop variables

  unsigned int order = pData_->order_;

  //build sVec (parametric vector for computation)
  BIAS::Vector<double>sVec(order);
  sVec[0]=0.;
  sVec[1]=1.;
  sVec[2]=2*s;
  double tmpS;
  for(i=3;i<order;i++){ //only go through loops if order>3
    tmpS=s;
    for(j=0;j<(i-2);j++){
      tmpS*=s;
    }
    tmpS*=i;
    sVec[i]=tmpS;
  }

  //compute result point
  res[0]=-sVec.ScalarProduct(curveVectors_[span][1]);
  res[1]= sVec.ScalarProduct(curveVectors_[span][0]);

  //normalise
  double length=res.NormL2();
  res[0]/=length;
  res[1]/=length;
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
GetFeature(const BIAS::Image<float>& featImg,
           const unsigned int normalWidth,
           const BIAS::Vector2<double>& curvePnt,
           const BIAS::Vector2<double>& normal,
           BIAS::Vector2<double>& res){
  unsigned int i;//counter variable

  //nothing found
  res[0]=curvePnt[0];
  res[1]=curvePnt[1];

  float maxInt;   //intensity
  float tmpInt;

  //check curvePnt
  if((curvePnt[0])<0 || (curvePnt[1]<0) || (curvePnt[0]>featImg.GetWidth()) ||
                                           (curvePnt[1]>featImg.GetHeight()) ){
    return;
  }
  maxInt = featImg.PixelValue((unsigned int)(curvePnt[0]+0.5),
                              (unsigned int)(curvePnt[1]+0.5));

  for(i=1;i<normalWidth; i++){
    //search outer region for change in pixelvalue
    int x = (int)(curvePnt[0]+(double)i*normal[0]+0.5);
    int y = (int)(curvePnt[1]+(double)i*normal[1]+0.5);

    if(x<(int)featImg.GetWidth() && y<(int)featImg.GetHeight() && x>=0 && y>=0){ //check img border
      tmpInt = featImg.PixelValue(x,y);
      if(tmpInt>maxInt){
        maxInt=tmpInt;
        res[0]=curvePnt[0]+(double)i*normal[0];
        res[1]=curvePnt[1]+(double)i*normal[1];
      }
    }

    //search inner region for change in pixelvalue
    x = (int)(curvePnt[0]-(double)i*normal[0]+0.5);
    y = (int)(curvePnt[1]-(double)i*normal[1]+0.5);
    if(x<(int)featImg.GetWidth() && y<(int)featImg.GetHeight() && x>=0 && y>=0){
      tmpInt = featImg.PixelValue(x,y);
      if(tmpInt>maxInt){
        maxInt=tmpInt;
        res[0]=curvePnt[0]-(double)i*normal[0];
        res[1]=curvePnt[1]-(double)i*normal[1];
      }
    }
  }
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
GetFeature(const BIAS::Image<float>& featImg,
           const unsigned int normalWidth,
           const BIAS::Vector2<double>& curvePnt,
           const BIAS::Vector2<double>& normal,
           const double& minClip,
           BIAS::Vector2<double>& res){
  unsigned int i;//counter variable

  //nothing found
  res[0]=curvePnt[0];
  res[1]=curvePnt[1];

  float maxInt;   //intensity
  float tmpInt;

  //check curvePnt
  if((curvePnt[0])<0 || (curvePnt[1]<0) || (curvePnt[0]>featImg.GetWidth()) ||
     (curvePnt[1]>featImg.GetHeight()) ){
    return;
  }
  maxInt = featImg.PixelValue((unsigned int)(curvePnt[0]+0.5),
                              (unsigned int)(curvePnt[1]+0.5));

  for(i=1;i<normalWidth; i++){
    //search outer region for change in pixelvalue
    int x = (int)(curvePnt[0]+(double)i*normal[0]+0.5);
    int y = (int)(curvePnt[1]+(double)i*normal[1]+0.5);

    if(x<(int)featImg.GetWidth() && y<(int)featImg.GetHeight() && x>=0 && y>=0){ //check img border
      tmpInt = featImg.PixelValue(x,y);
      if(tmpInt>maxInt && tmpInt>=minClip){
        maxInt=tmpInt;
        res[0]=curvePnt[0]+(double)i*normal[0];
        res[1]=curvePnt[1]+(double)i*normal[1];
      }
    }

    //search inner region for change in pixelvalue
    x = (int)(curvePnt[0]-(double)i*normal[0]+0.5);
    y = (int)(curvePnt[1]-(double)i*normal[1]+0.5);
    if(x<(int)featImg.GetWidth() && y<(int)featImg.GetHeight() && x>=0 && y>=0){
      tmpInt = featImg.PixelValue(x,y);
      if(tmpInt>maxInt && tmpInt>=minClip){
        maxInt=tmpInt;
        res[0]=curvePnt[0]-(double)i*normal[0];
        res[1]=curvePnt[1]-(double)i*normal[1];
      }
    }
  }
}


/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
Fit_(const BIAS::Image<float>& featImg, const double& alpha, const unsigned int normalWidth){
  unsigned int i; //counter variables
  double t; //counter variable

  unsigned int span;
  unsigned int numSpans=pData_->numSpans_;
  unsigned int numBasePolynoms=pData_->numBasePolynoms_;
  double s;

  BIAS::Vector2<double> curvePnt; //point on BSpline at time t
  BIAS::Vector2<double> normal;   //normal at time t
  BIAS::Vector2<double> featPnt;  //feature point at time t determined by filter

  BIAS::Vector<double> sVec(pData_->order_); //curve param vector at time t

  /*
   *  initialise Z_0 (?observation vector?) and
   *             S_0 (?statistical information matrix?)
   */
  unsigned int size=pShapeSpaceMatrix_->num_cols();
  BIAS::Vector<double> Z(size);
  BIAS::Matrix<double> S(size,size);
  BIAS::Matrix<double> tmpS(size, size);
  Z.SetZero();
  S.SetZero();

  //iteration step (core of algorithm)
  for(t=0.; t<(1.+*fitSampleWidth_); t+=*fitSampleWidth_){

    /*
     * use filter to find new position at time t
     * - get span at time t
     * - convert t(->s) relative to span
     * - get point and normal at time t
     * - get feature point
     */
    if(t<=0.){
      span=0;
      s=0.;
    }
    else if(t>=1.){
      span=numSpans-1;
      s=1.;
    }
    else{
      span = (unsigned int) (t * (double)(numSpans) );
      s= (t*(double)numSpans) - (double)span;
    }
    GetPointAndParamVec_(span,s,sVec,curvePnt);
    GetNormal_(span,s,normal);
    GetFeature(featImg, normalWidth, curvePnt, normal, featPnt);

    //1. compute v_i
    double v = (featPnt-curvePnt).ScalarProduct(normal);

    //2. compute h(si)^T
    BIAS::Vector<double> h(size);

    //2.1 compute U(si)
    BIAS::Vector<double> B(numBasePolynoms);
    (pData_->placedSpanMatrices_)[span].MultLeft(sVec, B);

    BIAS::Matrix<double> U(2,2*numBasePolynoms);
    U.SetZero();
    for(i=0;i<numBasePolynoms;i++){
      U[0][i]=B[i];
      U[1][i+numBasePolynoms]=B[i];
    }
    U.Mult(*pShapeSpaceMatrix_); //inplace multiplication!!!
    U.MultLeft(normal, h);//h(si)

    //3. update S
    S=S+((1./(double)fitSamples_)* h.OuterProduct(h));//TODO "measurement error"

    //4. update Z
    Z=Z+(((1./(double)fitSamples_)*v)*h);
  }

  //update control points
  BIAS::Matrix<double> regMat;
  pRegMatrix_->Multiply(alpha, regMat);
  BIAS::SVD superS(regMat+S);
  BIASCDOUT(D_CONTOURBSPLINE_FIT, "old Q_" << Q_ << std::endl << std::flush);
  Q_=*pShapeSpaceMatrix_*(superS.Invert()*Z) + Q_;
  BIASCDOUT(D_CONTOURBSPLINE_FIT, "new Q_" << Q_ << std::endl << std::flush);
  BIASCDOUT(D_CONTOURBSPLINE_FIT, "S^-: " << pRegMatrix_ << std::endl << std::flush);
  BIASCDOUT(D_CONTOURBSPLINE_FIT, "S: " << S << std::endl << std::flush);
  BIASCDOUT(D_CONTOURBSPLINE_FIT, "(S+S^-)^-1: " << superS.Invert() << std::flush);
  BIASCDOUT(D_CONTOURBSPLINE_FIT, "Z: " << Z << std::endl << std::flush);

  //through update of the control points the curveVectors_ aren't valid anymore
  curveInitialised_=false;
}

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
Fit_(const BIAS::Image<float>& featImg, const double& alpha,
     const unsigned int normalWidth, const double& minClip){
  unsigned int i; //counter variables
  double t; //counter variable

  unsigned int span;
  unsigned int numSpans=pData_->numSpans_;
  unsigned int numBasePolynoms=pData_->numBasePolynoms_;
  double s;

  BIAS::Vector2<double> curvePnt; //point on BSpline at time t
  BIAS::Vector2<double> normal;   //normal at time t
  BIAS::Vector2<double> featPnt;  //feature point at time t determined by filter

  BIAS::Vector<double> sVec(pData_->order_); //curve param vector at time t

  /*
   *  initialise Z_0 (?observation vector?) and
   *             S_0 (?statistical information matrix?)
   */
  unsigned int size=pShapeSpaceMatrix_->num_cols();
  BIAS::Vector<double> Z(size);
  BIAS::Matrix<double> S(size,size);
  BIAS::Matrix<double> tmpS(size, size);
  Z.SetZero();
  S.SetZero();

  //iteration step (core of algorithm)
  for(t=0.; t<(1.+*fitSampleWidth_); t+=*fitSampleWidth_){

    /*
     * use filter to find new position at time t
     * - get span at time t
     * - convert t(->s) relative to span
     * - get point and normal at time t
     * - get feature point
     */
    if(t<=0.){
      span=0;
      s=0.;
    }
    else if(t>=1.){
      span=numSpans-1;
      s=1.;
    }
    else{
      span = (unsigned int) (t * (double)(numSpans) );
      s= (t*(double)numSpans) - (double)span;
    }
    GetPointAndParamVec_(span,s,sVec,curvePnt);
    GetNormal_(span,s,normal);
    GetFeature(featImg, normalWidth, curvePnt, normal, minClip, featPnt);

    //1. compute v_i
    double v = (featPnt-curvePnt).ScalarProduct(normal);

    //2. compute h(si)^T
    BIAS::Vector<double> h(size);

    //2.1 compute U(si)
    BIAS::Vector<double> B(numBasePolynoms);
    (pData_->placedSpanMatrices_)[span].MultLeft(sVec, B);

    BIAS::Matrix<double> U(2,2*numBasePolynoms);
    U.SetZero();
    for(i=0;i<numBasePolynoms;i++){
      U[0][i]=B[i];
      U[1][i+numBasePolynoms]=B[i];
    }
    U.Mult(*pShapeSpaceMatrix_); //inplace multiplication!!!
    U.MultLeft(normal, h);//h(si)

    //3. update S
    S=S+((1./(double)fitSamples_)* h.OuterProduct(h));//TODO "measurement error"

    //4. update Z
    Z=Z+(((1./(double)fitSamples_)*v)*h);
  }

  //update control points
  BIAS::Matrix<double> regMat;
  pRegMatrix_->Multiply(alpha, regMat);
  BIAS::SVD superS(regMat+S);
  BIASCDOUT(D_CONTOURBSPLINE_FIT, "old Q_" << Q_ << std::endl << std::flush);
  Q_=*pShapeSpaceMatrix_*(superS.Invert()*Z) + Q_;
  BIASCDOUT(D_CONTOURBSPLINE_FIT, "new Q_" << Q_ << std::endl << std::flush);
  BIASCDOUT(D_CONTOURBSPLINE_FIT, "S^-: " << pRegMatrix_ << std::endl << std::flush);
  BIASCDOUT(D_CONTOURBSPLINE_FIT, "S: " << S << std::endl << std::flush);
  BIASCDOUT(D_CONTOURBSPLINE_FIT, "(S+S^-)^-1: " << superS.Invert() << std::flush);
  BIASCDOUT(D_CONTOURBSPLINE_FIT, "Z: " << Z << std::endl << std::flush);

  //through update of the control points the curveVectors_ aren't valid anymore
  curveInitialised_=false;
}

//private:

/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
GetFeatImage_(BIAS::Image<StorageType>& greyImg,
              BIAS::Image<float>& featImg,
              const unsigned int normalWidth){
  BIAS::Image<float> featX;
  BIAS::Image<float> featY;
  int minx,miny,maxx,maxy;
  GetBoundingBox(minx,miny,maxx,maxy);
  featImg.Init(greyImg.GetWidth(), greyImg.GetHeight(),
               greyImg.GetChannelCount());
  featImg.FillImageWithConstValue(0.);

  greyImg.SetROICorners(
    (minx>(int)normalWidth)? minx-normalWidth : 0,
    (miny>(int)normalWidth)? miny-normalWidth : 0,
    ((maxx+normalWidth)<greyImg.GetWidth())?
      maxx+normalWidth : (greyImg.GetWidth()-1),
    ((maxy+normalWidth)<greyImg.GetHeight())?
      maxy+normalWidth : (greyImg.GetHeight()-1));

  //filter ROI -> gradient image
  BIAS::GradientSobel3x3<StorageType, float> gsobel;
  gsobel.Filter(greyImg,featX,featY,featImg);
  greyImg.UnsetROI();
}



#ifdef BIAS_DEBUG
/////////////////////////////////////////////////////////
template <class StorageType> void
ContourDetectorBSpline<StorageType>::
TestInit_(const unsigned int order,
          const ContourBSplineType::Type bType,
          const std::vector<unsigned int>& mPnts,
          const BIAS::Vector<double>& Q){
  unsigned int i; //counter variable

 //test equality of dimension of control points and multiplicity vector
  if(mPnts.size()!=Q.Size()/2){
    BIASERR("Dimension of control point vector and dimension of multiplicity"<<
            " vector for control points arent equal.\r\n");
    exit(EXIT_FAILURE);
  }
  //test dimenstion of control points vector
  if(Q.Size()/2<order){
    BIASERR("BSpline needs >=" << order << " control points, even if one "<<
            "of the control points has a multiplicity >1.\r\n");
    exit(EXIT_FAILURE);
  }
  //avoid real multiple control points
  for(i=1;i<Q.Size()/2;i++){
    if((Q[i]==Q[i-1]) && (Q[i+Q.size()/2]==Q[i-1+Q.size()/2])){
      BIASERR("Dont use real multiple control points. Pontrol point " << i <<
              " and " << i+1 << " are equal. This would bug the contour " <<
              "algorithm. Try to increase the multiplicity for the control " <<
              "point.\r\n");
      exit(EXIT_FAILURE);
    }
  }

  /*
   * aperiodic BSpline tests
   */
  if(bType==ContourBSplineType::Open){
    //test aperiodic case for first and last controlpoint multiplicity
    if(mPnts.front()>1 || mPnts.back()>1){
      BIASWARN("This BSpline interpolates through end points. A "<<
               "multiplicity for first and last control point greater 1 "<<
               "gets ignored.\r\n");
    }
    //test aperiodic bspline for reasonable multiplicity
    for(i=1;i<order-1;i++){//check front
      if(mPnts[i]>1){
        BIASERR("The first control point with multiplicity greater 1 may be "<<
                "control point "<< order-1 << "(begin counting with 0).\r\n");
        exit(EXIT_FAILURE);
      }
    }
    for(i=mPnts.size()-2;i>mPnts.size()-order;i--){//check back
      if(mPnts[i]>1){
        BIASERR("the last control point with multiplicity greater 1 may be " <<
                "control point " << mPnts.size()-order <<
                "(begin counting with 0).\r\n");
        exit(EXIT_FAILURE);
      }
    }
    //test aperiodic bspline for splits
    for(i=order-1;i<mPnts.size()-order;i++){
      if(mPnts[i]>=order){
        BIASERR("The multiplicity of control point " << i <<
                " (begin counting with 0) is >=" << order <<
                " and would thus split the B-SPline curve into parts.\r\n");
        exit(EXIT_FAILURE);
      }
    }
  }

  /*
   * periodic BSpline tests
   */
  if(bType==ContourBSplineType::Closed){
    //test periodic bspline for splits
    for(i=0;i<mPnts.size();i++){
      if(mPnts[i]>=order){
        BIASERR("The multiplicity of " << i << "th control point is " <<
                ">=" << order << " and would thus split the BSpline into " <<
                "parts.\r\n");
        exit(EXIT_FAILURE);
      }
    }
  }
}
#endif

//////////////////////////////////////////////////////////////////////////
//                 instantiation
//////////////////////////////////////////////////////////////////////////
namespace BIAS{
template class ContourDetectorBSpline<unsigned char>;
//template class ContourDetectorBSpline<float>;

// fill in instances as required
#ifdef BUILD_IMAGE_INT
template class ContourDetectorBSpline<int>;
#endif
#ifdef BUILD_IMAGE_CHAR
//template class ContourDetectorBSpline<char>;
#endif
#ifdef BUILD_IMAGE_SHORT
#endif
#ifdef BUILD_IMAGE_USHORT
template class ContourDetectorBSpline<unsigned short>;
#endif
#ifdef BUILD_IMAGE_UINT
#endif
#ifdef BUILD_IMAGE_DOUBLE
#endif
}