PMDImageProc.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/PMDImageProc.hh>
#include <Geometry/RMatrix.hh>
#include <Geometry/Projection.hh>
#include <Image/ProjectionMapping.hh>
#include <Image/HomographyMapping.hh>
#include <Base/Image/ImageIO.hh>
#include <Filter/Dilation.hh>
#include <Filter/Median.hh>
#include <bias_config.h>
#include <stdio.h>
#include <Base/Math/Random.hh>
#include <MathAlgo/SVD.hh>
#include <MathAlgo/Histogram1D.hh>

using namespace BIAS;
using namespace std;

int PMDImageProc::FitDepthTo2DImage(Image<float> &ImageDepth,
                                    const BIAS::Projection &proj2D,
                                    const BIAS::Projection &projDepth,
                                    int kernelsize)
{
  Image<float> ImgDepthTo2D;
  unsigned int depthWidth=0,depthHeight=0;
  depthWidth= ImageDepth.GetWidth();
  depthHeight= ImageDepth.GetHeight();

  unsigned int twoDWidth=0, twoDHeight=0;
  proj2D.GetParameters()->GetImageSize(twoDWidth,twoDHeight);
  ImgDepthTo2D.Init(twoDWidth,twoDHeight, 1);
  ImgDepthTo2D.SetZero();

  float **idaDepth = ImageDepth.GetImageDataArray();
  float **idaDepthTo2D = ImgDepthTo2D.GetImageDataArray();

  HomgPoint2D posOld(0,0,0), maxPosDiff(0,0,0);
  double depthold=0;

  for (unsigned int x=0; x< depthWidth; x++) {
    for (unsigned int y=0; y< depthHeight; y++) {
      Vector3<double> p3D;
      HomgPoint2D pos(x,y,1); // this is 2D point on depth image
      float depth = idaDepth[y][x]; //depth value
      if (depth>0.0) {
        //now unproject to 3D point
        p3D = projDepth.UnProjectToPoint(pos, depth);

        HomgPoint3D hp3D(p3D);
        // now calculate the 2D point on 2D image
        pos = proj2D.Project(hp3D);      
        unsigned int xx=(unsigned int)rintf(pos[0]);
        unsigned int yy=(unsigned int)rintf(pos[1]);

        if (xx>=0 && xx < twoDWidth && yy >= 0 && yy < twoDHeight) {
          idaDepthTo2D[yy][xx] = depth;
        
          if (y>0 && posOld.NormL2()>0 && fabs((depth-depthold)/depth)<0.001) {
            if (maxPosDiff.NormL2()<(posOld-pos).NormL2()) {
              maxPosDiff = posOld-pos;
            }
          }
          posOld = pos;
          depthold = depth;
        } else {
          posOld[0]=0;
          posOld[1]=0;
          posOld[2]=0;
          depthold = 0;
        }
      } else {
        depthold = 0;
      }
    }
    posOld[0]=0;
    posOld[1]=0;
    posOld[2]=0;
  }

  Image<float> res;
  Dilation<float, float> dilation;

  //if no kernel size is given calculate the size
  if(kernelsize == 0){
    kernelsize=int(ceil(maxPosDiff.NormL2()));
    if (kernelsize==0 || kernelsize>60) {
        BIASERR("Calculation of Kernelsize did not work, calculated "
                << kernelsize);
        kernelsize=10;
    } else {
        kernelsize = kernelsize/2 +1;
    }
  }
  cout<<"Kernelsize:"<<kernelsize<<endl;
  dilation.SetKernelSize(kernelsize);
  // kernelsize 3 wich is default is MUCH faster
  dilation.SetBorderHandling(FilterBase<float, float>::TBH_same);
  dilation.Filter(ImgDepthTo2D,res);  
  
  ImageDepth = res;

  return 0;
}




int PMDImageProc::FitDepthTo2DImage(Image<float> &ImgDepth, 
                                    const PMDImageMetaData &MetaData2D,
                                    PMDImageMetaData &MetaDataDepth)
{
  Image<float> ImgMod;
  int ret;
  ImgMod = ImgDepth;
  if (!Equal(MetaData2D.CameraCenter[0], MetaDataDepth.CameraCenter[0],1e-3) ||
      !Equal(MetaData2D.CameraCenter[1], MetaDataDepth.CameraCenter[1],1e-3) ||
      !Equal(MetaData2D.CameraCenter[2], MetaDataDepth.CameraCenter[2],1e-3)) 
  {
    ret =  FitDepthTo2DImageFree_(ImgMod, ImgDepth, MetaData2D, MetaDataDepth);
  } else {
    if (!Equal(MetaData2D.CameraRotation[0], 
               MetaDataDepth.CameraRotation[0],1e-3) ||
        !Equal(MetaData2D.CameraRotation[1], 
               MetaDataDepth.CameraRotation[1],1e-3) ||
        !Equal(MetaData2D.CameraRotation[2], 
               MetaDataDepth.CameraRotation[2],1e-3)) 
    {
      ret = FitDepthTo2DImageHomogr_(ImgMod, MetaData2D, MetaDataDepth);
    } else {
      ret = FitDepthTo2DImageKOnly_(ImgMod, MetaData2D, MetaDataDepth);
    }
  }
  ImgDepth = ImgMod;
  return ret;

}

int PMDImageProc::FitModCoeffTo2DImage(Image<float> &ImgModCoeff, 
                                       const Image<float> &ImgDepth,
                                       const PMDImageMetaData &MetaData2D,
                                       PMDImageMetaData &MetaDataDepth) 
{
  int ret;
  if (!Equal(MetaData2D.CameraCenter[0], MetaDataDepth.CameraCenter[0],1e-3) ||
      !Equal(MetaData2D.CameraCenter[1], MetaDataDepth.CameraCenter[1],1e-3) ||
      !Equal(MetaData2D.CameraCenter[2], MetaDataDepth.CameraCenter[2],1e-3)) 
  {
    ret = FitDepthTo2DImageFree_(ImgModCoeff, ImgDepth,
                                 MetaData2D, MetaDataDepth);
  } else {
    if (!Equal(MetaData2D.CameraRotation[0], 
               MetaDataDepth.CameraRotation[0],1e-3) ||
        !Equal(MetaData2D.CameraRotation[1], 
               MetaDataDepth.CameraRotation[1],1e-3) ||
        !Equal(MetaData2D.CameraRotation[2], 
               MetaDataDepth.CameraRotation[2],1e-3)) 
    {
      ret = FitDepthTo2DImageHomogr_(ImgModCoeff, MetaData2D, MetaDataDepth);
    } else {
      ret = FitDepthTo2DImageKOnly_(ImgModCoeff, MetaData2D, MetaDataDepth);
    }
  }
  return ret;
}

int PMDImageProc::FitDepthTo2DImageKOnly_(Image<float> &ImgToModify,
                                          const PMDImageMetaData &MetaData2D,
                                          PMDImageMetaData &MetaDataDepth) 
{
  HomographyMapping<float, float> hm;
  
  KMatrix K2D, KDepth;
  K2D = KFromMeta(MetaData2D);
  KDepth =  KFromMeta(MetaDataDepth);
 
  HMatrix H = KDepth*K2D.Invert();;
  int ret = 0;
  if (!H.IsIdentity()) {
    hm.SetHomography(H);
    Image<float> mapto(MetaData2D.width, MetaData2D.height, 1);
    ret = hm.Map(ImgToModify, mapto);
    ImgToModify = mapto;
    MetaDataDepth = MetaData2D;
  } else ret = -1;
  return ret;
}

int PMDImageProc::FitDepthTo2DImageFree_(Image<float> &ImgToModify,
                                         const Image<float> &ImgDepth, 
                                         const PMDImageMetaData &MetaData2D,
                                         PMDImageMetaData &MetaDataDepth) 
{
  BIASWARN("This is only a rough approximation better performance in derived" <<
           "class 'GLFilter/PMDImageProcGL' (class unimplemented ischiller 04/07)");
  Projection proj2D, projDepth;
  vector<double> undistDepth(4,0.0);
  undistDepth[0] = MetaDataDepth.UndistCoeff[0];
  undistDepth[1] = MetaDataDepth.UndistCoeff[1];
  undistDepth[2] = MetaDataDepth.UndistCoeff[2];
  undistDepth[3] = MetaDataDepth.UndistCoeff[3];

  vector<double> undist2D(4,0.0);
  undist2D[0] =MetaData2D.UndistCoeff[0];
  undist2D[1] =MetaData2D.UndistCoeff[1];
  undist2D[2] =MetaData2D.UndistCoeff[2];
  undist2D[3] =MetaData2D.UndistCoeff[3];
  BIAS_ProjParaPersp_DISTORTION_TYPE radDistType=DISTYPE_DEF;

  projDepth.CreatePerspective(PMDImageProc::PoseFromMeta(MetaDataDepth), 
                              PMDImageProc::KFromMeta(MetaDataDepth),
                              MetaDataDepth.width, MetaDataDepth.height,
                              radDistType, undistDepth);
  proj2D.CreatePerspective(PMDImageProc::PoseFromMeta(MetaData2D), 
                           PMDImageProc::KFromMeta(MetaData2D),
                           MetaData2D.width, MetaData2D.height,
                           radDistType,undist2D);
      
  Image<float> ImgDepthTo2D;
  ImgDepthTo2D.Init(MetaData2D.width, MetaData2D.height, 1);
  ImgDepthTo2D.SetZero();
  HomgPoint2D posOld(0,0,0), maxPosDiff(0,0,0);
  double depthold=0;
  for (unsigned int x=0; x<ImgDepth.GetWidth(); x++) {
    for (unsigned int y=0; y<ImgDepth.GetHeight(); y++) {
      Vector3<double> p3D;
      HomgPoint2D pos(x,y,1);
      float depth = ImgDepth.PixelValue(x,y);
      if (depth>0) {
        p3D = projDepth.UnProjectToPoint(pos, depth);
        //cout << "pos" << pos << " depth"<<depth<< "  3D:" << p3D;
        HomgPoint3D hp3D(p3D);
        pos = proj2D.Project(hp3D);
        //cout << "pos" << pos <<endl;
        int xx=(int)rintf(pos[0]);
        int yy=(int)rintf(pos[1]);
        if (xx>=0 && xx<(int)ImgDepthTo2D.GetWidth() && 
            yy>=0 && yy<(int)ImgDepthTo2D.GetHeight()) {
          //ImgDepthTo2D.SetPixel(float((proj2D.GetC()-p3D).NormL2()), xx, yy);
          ImgDepthTo2D.SetPixel(ImgToModify.PixelValue(x,y), xx, yy);
         if (y>0 && posOld.NormL2()>0 && fabs((depth-depthold)/depth)<0.001) {
            if (maxPosDiff.NormL2()<(posOld-pos).NormL2()) {
              maxPosDiff = posOld-pos;
            }
          }
          posOld = pos;
          depthold = depth;
        } else {
          posOld[0]=0;
          posOld[1]=0;
          posOld[2]=0;
          depthold = 0;
        }
      } else {
          depthold = 0;
      }
    }
    posOld[0]=0;
    posOld[1]=0;
    posOld[2]=0;
  }
  //ImageIO::Save("warpeddepth.mip",ImgDepthTo2D);
  Image<float> res;
  Dilation<float, float> dilation;
  int kernelsize=int(ceil(maxPosDiff.NormL2()));
  if (kernelsize==0 || kernelsize>25) {
    BIASERR("Calculation of Kernelsize did not work, calculated " << kernelsize);
    kernelsize=3;
  } else {
    kernelsize = kernelsize/2*2+1;
  }
  dilation.SetKernelSize(kernelsize); // kernelsize 3 wich is default is MUCH faster
  dilation.SetBorderHandling(FilterBase<float, float>::TBH_same);
  dilation.Filter(ImgDepthTo2D,res);  
  ImgToModify = res;

  return 0;

}

int PMDImageProc::FitDepthTo2DImageHomogr_(Image<float> &ImgToModify,
                                           const PMDImageMetaData &MetaData2D,
                                           PMDImageMetaData &MetaDataDepth) {

  HomographyMapping<float, float> hm;
  HMatrix H = HFromMeta_(MetaData2D, MetaDataDepth);
  int ret = 0;
  if (!H.IsIdentity()) {
    hm.SetHomography(H);
    Image<float> mapto(MetaData2D.width, MetaData2D.height, 1);
    ret = hm.Map(ImgToModify, mapto);
    if (ret>=0) {
      ImgToModify = mapto;
      MetaDataDepth = MetaData2D;
    }
  } else ret = -1;
  return ret;
}


void PMDImageProc::
Fit2DToDepthImage(Image<float> &depthImage,
                                  Image<unsigned char>& textureImage,
                                  Projection& depthProj,Projection& textureProj,
                                  Image<unsigned char>& newColorImage)
{
    unsigned w = depthImage.GetWidth(), h = depthImage.GetHeight();
    unsigned c = textureImage.GetChannelCount();
    if(newColorImage.IsEmpty()) newColorImage.Init(w,h,c    );
    else if(newColorImage.GetWidth() != w || newColorImage.GetHeight() != h ||
            newColorImage.GetChannelCount() != c) newColorImage.ReInit(w,h,c);

    unsigned char color[3]={0,0,0};
    newColorImage.FillImageWithConstValue(color);

    float** idaD = depthImage.GetImageDataArray();
    unsigned char** idaC = newColorImage.GetImageDataArray();
    unsigned char** idaOldC = textureImage.GetImageDataArray();
    for(unsigned y=0;y<h;y++){
        for(unsigned x=0;x<w;x++){
            if(idaD[y][x] != 0.0){
                double depth = idaD[y][x];
                Vector3<double> p = depthProj.UnProjectToPoint(HomgPoint2D(x,y,1),depth);
                HomgPoint2D p2D;
                if(textureProj.DoesPointProjectIntoImage(HomgPoint3D(p),p2D)){
                    for(unsigned i=0;i<c;i++){
                        idaC[y][x*c+i] = idaOldC[unsigned(p2D[1])][(unsigned)(p2D[0])*c+i];
                    }
                }
            }
        }
    }
}


HMatrix PMDImageProc::HFromMeta_(const PMDImageMetaData &MetaData2D, 
                                 const PMDImageMetaData &MetaDataDepth) {

  if (!Equal(MetaData2D.CameraCenter[0], MetaDataDepth.CameraCenter[0],1e-3) ||
      !Equal(MetaData2D.CameraCenter[1], MetaDataDepth.CameraCenter[1],1e-3) ||
      !Equal(MetaData2D.CameraCenter[2], MetaDataDepth.CameraCenter[2],1e-3)) 
  {
      BIASERR("Can not Fit Depth Image to 2D Image, centers are not equal!!");
      return BIAS::HMatrix(MatrixIdentity);
  }
  KMatrix K2D, KDepth;
  K2D = KFromMeta(MetaData2D);
  KDepth =  KFromMeta(MetaDataDepth);
  
  Vector3<double> rot2D(MetaData2D.CameraRotation[0],
                        MetaData2D.CameraRotation[1],
                        MetaData2D.CameraRotation[2]);
  Vector3<double> rotDepth(MetaDataDepth.CameraRotation[0],
                           MetaDataDepth.CameraRotation[1],
                           MetaDataDepth.CameraRotation[2]);
  RMatrix RDepth, R2D;
  RDepth.SetFromAxisAngle(rotDepth);
  R2D.SetFromAxisAngle(rot2D);
  HMatrix H = KDepth*RDepth.Transpose()*R2D*K2D.Invert();
  return H;

}


KMatrix PMDImageProc::KFromMeta(const PMDImageMetaData &MetaData) {
  KMatrix K;
  K.SetIdentity();
  K[0][0] = MetaData.FocallengthX;
  K[1][1] = MetaData.AspectRatio * MetaData.FocallengthX;
  K[0][2] = MetaData.PrincipalX;
  K[1][2] = MetaData.PrincipalY;
  return K;
} 


Pose PMDImageProc::PoseFromMeta(const PMDImageMetaData &MetaData) {
  Pose pose;
  RMatrix R;
  R.SetFromAxisAngle(Vector3<double>(MetaData.CameraRotation));
  Quaternion<double> Q;
  R.GetQuaternion(Q);
  pose.SetQ(Q);
  pose.SetC(Vector3<double>(MetaData.CameraCenter));
  return pose;
}


PMDImageMetaData PMDImageProc::MetaFromPoseAndK(const Pose &Po,
                                                const KMatrix &K, int width, 
                                                int height, int channels) 
{
  PMDImageMetaData MetaData;
  Vector3<double> axis, C;
  double angle;
  Po.GetQ().GetAxisAngle(axis, angle);  
  axis.Normalize();
  axis = axis * angle;
  MetaData.CameraRotation[0] = axis[0];
  MetaData.CameraRotation[1] = axis[1];
  MetaData.CameraRotation[2] = axis[2];
  C = Po.GetC();
  MetaData.CameraCenter[0] = C[0];
  MetaData.CameraCenter[1] = C[1];
  MetaData.CameraCenter[2] = C[2];

  MetaData.FocallengthX = K[0][0];
  MetaData.AspectRatio = K[1][1] / MetaData.FocallengthX;
  MetaData.PrincipalX = K[0][2];
  MetaData.PrincipalY = K[1][2];

  MetaData.UndistCoeff[0] = 0;
  MetaData.UndistCoeff[1] = 0;
  MetaData.UndistCoeff[2] = 0;
  MetaData.UndistCoeff[3] = 0;

  MetaData.width = width;
  MetaData.height = height;
  MetaData.channels = channels;

  return MetaData;
}

void PMDImageProc::
LimitDepthSpread(BIAS::Image<float>& depthImage,
                                 float maxDeviation)
{
    unsigned w = depthImage.GetWidth();
    unsigned h = depthImage.GetHeight();
    float** ida = depthImage.GetImageDataArray();
    float mean=0.0; int counter=0;

    float min=100000.0f,max=0.0f;
    vector<float> histogramData;
    ////////// analyze image with histogram /////////////
    for(unsigned y=0;y<h;y++){
        for(unsigned x=0;x<w;x++){
            if (ida[y][x] == 0) {
                continue;
            }

            if(ida[y][x] < min) min = ida[y][x];
            if(ida[y][x] > max) max = ida[y][x];
            mean += ida[y][x];
            counter++;
            histogramData.push_back(ida[y][x]);
        }
    }

    if(counter > 0){
        Histogram1D<float> hist;
        hist.SetHistogramBoundaries(min,max);
        hist.SetNumBins(30);
        mean/=counter;

        //build up histogram
        hist.AddData(histogramData);
        //Get Maximum Value (bin with maximum number of entries)
        float depthAccumulationValue = hist.GetBinCenterWithMaxEntries();

        // now go over all pixel and limit spread to
        // histogramCenter+- maxDeviation
        for(unsigned y=0;y<h;y++){
            for(unsigned x=0;x<w;x++){
                if(ida[y][x] != 0.0){
                    if(ida[y][x] > depthAccumulationValue+maxDeviation) {
                        ida[y][x] = depthAccumulationValue+maxDeviation;
                    } else if (ida[y][x] < depthAccumulationValue-maxDeviation) {
                        ida[y][x] = depthAccumulationValue-maxDeviation;
                    }
                }
            }
        }
    }//end if(counter>0)
}

int PMDImageProc::DenoiseDepthImage(Image<float> &ImgDepth,
                                    int size,
                                    double sigmaVarianz) {
  Image<float> res;
  Median<float, float> median;
  median.SetSize(size); // 1 = 3x3, 2=5x5
  median.SetBorderHandling(FilterBase<float, float>::TBH_same);
  median.Filter(ImgDepth, res);  
  DeleteHighVarianceValues(res, sigmaVarianz,size);
  ImgDepth = res;
  return 0;
}
    

int PMDImageProc::AddNoiseToDepthImage(Image<float> &ImgDepth) {
  
  Random rnd;
  Image<float> ImgIn=ImgDepth;
  for (unsigned int x=0; x<ImgDepth.GetWidth(); x++) {
    for (unsigned int y=0; y<ImgDepth.GetHeight(); y++) {
      if (ImgDepth.PixelValue(x, y)!=0) {
        double sigma = PMDImageProc::CalcSigmaDepth(ImgIn, x, y, 1); 
        if (sigma>0) {
          float pixval = ImgIn.PixelValue(x, y);
          pixval = (float)rnd.GetNormalDistributed(pixval, sigma);
          ImgDepth.SetPixel(pixval,x, y);
        }
      }
    }
  }
  return 0;
}


void PMDImageProc::ScaleDepthImage(Image<float> &ImgDepth, 
                                   double scaleFactor) {
  
  for (unsigned int x=0; x<ImgDepth.GetWidth(); x++) {
    for (unsigned int y=0; y<ImgDepth.GetHeight(); y++) {
      ImgDepth.SetPixel(ImgDepth.PixelValue(x, y) * (float)scaleFactor, x, y);
    }
  }
  
}


double PMDImageProc::CalcSigmaDepth(const Image<float> &ImgDepth, 
                                    const unsigned int x, 
                                    const unsigned int y,
                                    const unsigned int HalfWinSize) 
{ 
  if (x<HalfWinSize || x>ImgDepth.GetWidth()-HalfWinSize-1 ||
      y<HalfWinSize || y>ImgDepth.GetHeight()-HalfWinSize-1) {
    return -1;
  }

  // Surrounding min and max depth in m
  double minDepth = 1000.0;
  double maxDepth = 0.0;

  for (int dx=-1*(int)HalfWinSize; dx<=int(HalfWinSize); dx++) {
    for (int dy=-1*(int)HalfWinSize; dy<=int(HalfWinSize); dy++) {
      if (ImgDepth.PixelValue(x+dx, y+dy)<minDepth &&
          ImgDepth.PixelValue(x+dx, y+dy)!=0) 
        minDepth = ImgDepth.PixelValue(x+dx, y+dy);
      if (ImgDepth.PixelValue(x+dx, y+dy)>maxDepth) 
        maxDepth = ImgDepth.PixelValue(x+dx, y+dy);
    }
  }
  // Standard deviation for a certain depth in meter
  //where do these values come from ?? (ischiller)
  double minDev= 0.002734*minDepth*minDepth + 
    0.00286723*minDepth - 0.0004229692;
  double maxDev= 0.002734*maxDepth*maxDepth + 
    0.00286723*maxDepth - 0.0004229692;

  //depth value is in this range
  minDepth -= minDev;
  maxDepth += maxDev;
  return (maxDepth-minDepth)/2;
}


void PMDImageProc::DeleteHighVarianceValues(BIAS::Image<float> &img,
                                            const double sigmaThresh,
                                            const int halfwinSize)
{
  BIAS::Image<float> res=img;
  float **data = img.GetImageDataArray();
  int cols=(int)img.GetWidth();
  int rows=(int)img.GetHeight();
  double var,diff;
  double count=0;
  for (int row=0; row<rows; row++)
    for (int col=0; col<cols; col++) {
        var=0;
        count=0;
        for ( int i=-halfwinSize; i<=halfwinSize; i++){
            for (int j=-halfwinSize; j<=halfwinSize; j++){
                if ( ((row+i)>=0) && ((col+j)>=0)  &&
                        ((row+i)<rows) && ((col+j)<cols) )
                {
                    diff= data[row+i][col+j]-data[row][col];
                    var+= diff*diff;
                    count++;
                }
            }
        }
        var/= count;
        if (sqrt(var)>sigmaThresh) res.GetImageDataArray()[row][col]=0;
    }
  img=res;
}

void PMDImageProc::MarkHighVarianceValues(const BIAS::Image<float> &img,
    BIAS::Image<float> &mark, double sigmaThresh,   int halfwinSize)
{
  const float **data = img.GetImageDataArray();
  float **mdata = mark.GetImageDataArray();
  
  int cols=(int)img.GetWidth();
  int rows=(int)img.GetHeight();
  if((int)mark.GetWidth() != cols || (int)mark.GetHeight()!= rows)
    mark.ReInit(cols,rows,1);
  double var,diff;
  double count=0;
  for (int row=0; row<rows; row++)
    for (int col=0; col<cols; col++) {
      var=0;
      count=0;
      for ( int i=-halfwinSize; i<halfwinSize; i++)
        for (int j=-halfwinSize; j<halfwinSize; j++) 
          if ( ((row+i)>=0) && ((col+j)>=0)  &&
               ((row+i)<rows) && ((col+j)<cols) )
            {
              diff= data[row+i][col+j]-data[row][col];
              var+= diff*diff;
              count++;
            }     
      var/= count;      
      if (sqrt(var)>sigmaThresh) mdata[row][col]=float(var);
      else mdata[row][col]=0.0f;
    }
}


int PMDImageProc::
CrossCheck(BIAS::Image<float>& cIm, const BIAS::Projection& cP,
                     std::vector<const BIAS::Image<float>* >& cImCheck,
                     std::vector<const Projection*>& cPCheck,
               int searchSize)
{
  unsigned w=0,h=0;
  int counter=0;
  w = cIm.GetWidth();
  h = cIm.GetHeight();
  //c = cIm.GetChannelCount();

  // get projection parameters
  float** ida = cIm.GetImageDataArray();

  for(unsigned y=0;y<h;y++){
    for(unsigned x=0;x<w;x++){

        if(ida[y][x] != 0){
            HomgPoint3D point3D = cP.GetParameters()->UnProjectToPoint(HomgPoint2D(x,y,1),ida[y][x]);

            vector<bool> isIn;
            for(unsigned i=0;i<cPCheck.size();i++){
                isIn.push_back(false);
                HomgPoint2D point;
                bool projects = false;
                projects = cPCheck[i]->DoesPointProjectIntoImage(point3D,point );

                if(projects && point[2]!=0){
                    unsigned  w2,h2,width,height;
                    cPCheck[i]->GetParameters()->GetImageSize(w2,h2);
                    width = cImCheck[i]->GetWidth();
                    height = cImCheck[i]->GetHeight();
                    if(w2 != width || h2 != height) {
                        BIASERR("Unmatching image dimensions between projection and image");
                        return -1;
                    }

                    int cX = (int) rint(point[0]);
                    int cY = (int) rint(point[1]);

                    //if this point is not 0 in one of the other images invalidate
                    const float** idaCheck = cImCheck[i]->GetImageDataArray();
                    if (idaCheck[cY][cX] != 0.0) {
                        isIn[i] = true;
                    }
                    else //search around the point
                    {
                        bool found = false;
                        for (int runX = cX - searchSize; runX <  (cX + searchSize); runX++) {
                            for (int runY = cY - searchSize; runY < (cY + searchSize); runY++) {
                                if (runX >= 0 && runY >= 0 && runX < (int)w2 && runY< (int)h2) {
                                    if (idaCheck[runY][runX] != 0) {
                                        isIn[i] = true;
                                        found = true;
                                        break;
                                    }
                                }
                            } //end for Y
                            if (found)  break;
                        }//end for X
                    }//end else
                }//end if(projects && point[2]!=0)
                else isIn[i] = false;
            }// end for(unsigned i=0;i<cPCheck();i++)
            bool bMustBeValidInAll=true;
            if(bMustBeValidInAll){
                for(unsigned i=0;i<isIn.size();i++) {
                    if(!isIn[i]){
                        ida[y][x] = 0.0;
                        counter++;
                        break;
                    }
                }
            }
            else{
                bool valid = false;
                for(unsigned i=0;i<isIn.size();i++) {
                    valid &= isIn[i];
                }
                if(!valid){
                    ida[y][x] = 0.0;
                    counter++;
                    break;
                }
            }
        }//end for x
    }//end for y
  }

  return counter;
}