Smoothing Kinect Depth Data

Archana11

Hello everyOne,

http://www.codeproject.com/Articles/317974/KinectDepthSmoothing,, from this post, m trying to make smooth kinect depth data by implementing the code in processing, But it's not giving me an improving result.

I know , I made a several mistakes , but cant make them working.

I have created a method of -createImageFromDepthArray- and -createSmoothImageFromDepthArray- , but I can not display the image using this methods...

Here is my code:

import SimpleOpenNI.*;

import java.util.Queue;
import java.util.ArrayDeque;
SimpleOpenNI kinect;
public Queue<int[]> averageQueue=new ArrayDeque();
boolean useFiltering;
int innerBandThreshold;
int outerBandThreshold;
boolean userAverage;
int averageFrameCount;

int totalFrames;
int lastFrames;

int RedIndex=2;
int GreenIndex=1;
int BlueIndex=0;

int MaxDepthDistance=4000;
int MinDepthDistance=850;
int MaxDepthDistanceOffset=3150;
PImage r_image;
void setup(){
  size(320,240);
  kinect=new SimpleOpenNI(this);
  kinect.enableDepth();
  kinect.enableRGB();

}
void draw(){
  kinect.update();
  r_image=kinect.depthImage();
  createImageFromDepthImage(r_image);
  createSmoothImageFromDepthArray(r_image);
  image(r_image,0,0);
}
public int CalculateDistanceFromDepth(int first, int second){
  return first|second<<8;
}
public byte CalculateIntensityFromDistance(int distance){
  int newMax=distance-MinDepthDistance;
  if(newMax>0){
    return (byte)(255-(255*newMax/(MaxDepthDistanceOffset)));
  }else{
    return (byte)255;
  }
}
public int[] createDepthArray(PImage image){
 int[] returnArray=new int[image.width*image.height];
 //byte[] depthFrame=kinect.depthMap();
 int[] depthFrame=kinect.depthMap();

 for(int y=0; y<640; y+=2){
   for(int x=0; x<240; x++){
     int depthIndex=y+(x*640);
     int index=depthIndex/2;

     returnArray[index]=CalculateDistanceFromDepth(depthFrame[depthIndex],depthFrame[depthIndex+1]);
   }
 }
 return returnArray;
}


public int[] ceateAverageDepthArray(int[] depthArray){
 averageQueue.add(depthArray);

 CheckForDequeue();

 int[] sumDepthArray=new int[depthArray.length];
 int[] averageDepthArray=new int[depthArray.length];

 int denominator=0;
 int count=1;

  for(int[] item : averageQueue)
   for(int y=0;y<320;y++){
     for(int x=0;x<240;x++){
       int index=y+(x*320);
       sumDepthArray[index]+=item[index]*count;

     }
   }

    denominator+=count;
    count++;

// }

 for(int y1=0;y1<320;y1++){
     for(int x1=0;x1<240;x1++){
       int index1=y1+(x1*320);
       averageDepthArray[index1]=(short)(sumDepthArray[index1]/denominator);

     }
   }

   return averageDepthArray;




}

public void CheckForDequeue(){
  if(averageQueue.size()>averageFrameCount){
    averageQueue.remove();
    CheckForDequeue();
  }
}

public int[] createFilteredDepthArray(int[] depthArray, int width, int height){

  int[] smoothDepthArray=new int[depthArray.length];

  int widthBound=width-1;
  int heightBound=height-1;

  for(int y2=0;y2<320;y2++){
    for(int x2=0;x2<240;x2++){
    int depthIndex=y2+(x2*320);
    if(depthArray[depthIndex]==0){
      int a=depthIndex%320;
      int b=(depthIndex-a)/320;

      int[][]filterCollection=new int[24][2];

      int innerBandCount=0;
      int outerBandCount=0;

      for(int yi=-2; yi<3; yi++ ){
        for(int xi=-2; xi<3;xi++){
          if(xi !=0 || yi!=0){
            int xSearch=a+xi;
            int ySearch=b+yi;

            if(xSearch>=0 && xSearch<=widthBound && ySearch>=0 && ySearch<=heightBound){
              int index_s=xSearch+(ySearch*width);
              if(depthArray[index_s]!=0){
                for(int i=0;i<24;i++){
                  if(filterCollection[i][0]==depthArray[index_s]){
                    filterCollection[i][1]++;
                    break;
                  }else if(filterCollection[i][0]==0){
                    filterCollection[i][1]=depthArray[index_s];
                    filterCollection[i][1]++;
                  }
                }
                if(yi!=2 && yi!=-2 && xi!=2 && xi!=-2){
                  innerBandCount++;
                }else{
                  outerBandCount++;
                }
              }
            }
          }
        }

        if(innerBandCount>=innerBandThreshold || outerBandCount>=outerBandThreshold){
          int frequency=0;
          int depth=0;

          for(int i=0;i<24;i++){
            if(filterCollection[i][0]==0){
              break;
            }
            if(filterCollection[i][1]>frequency){
              depth=filterCollection[i][0];
              frequency=filterCollection[i][0];
            }
          }

          smoothDepthArray[depthIndex]=depth;
        }else{
          smoothDepthArray[depthIndex]=depthArray[depthIndex];
        }
      }

    }
  }

}
        return smoothDepthArray;

}


public PImage createImageFromDepthImage(PImage image){
  int width=image.width;
  int height=image.height;

 int[] depthFrame=kinect.depthMap();
 int[] colorFrame = new int[width * height * 4];

 for(int y=0;y<640;y+=2){
   for(int x=0;x<240;x++){
     int depthIndex=y+(x*640);

     int index=depthIndex*2;
     int distance=CalculateDistanceFromDepth(depthFrame[depthIndex],depthFrame[depthIndex+1]);
     int intensity=CalculateIntensityFromDistance(distance);

     colorFrame[index+BlueIndex]=intensity;
     colorFrame[index+GreenIndex]=intensity;
     colorFrame[index+RedIndex]=intensity;
   }
 }
 return image;

}

public PImage createSmoothImageFromDepthArray(PImage image){
  int width=image.width;
  int height=image.height;

  int[] depthArray=createDepthArray(image);
  if(useFiltering){
    depthArray=createFilteredDepthArray(depthArray,width,height);
  }
  if(userAverage){
    depthArray=ceateAverageDepthArray(depthArray);
  }
  int[] colorBytes=createColorBytesFromDepthArray(depthArray,width,height);
  return image;
}

public int[] createColorBytesFromDepthArray(int[] depthArray,int width,int height){
  int[] colorFrame=new int[width*height*4];

  for(int y=0;y<320;y++){
    for(int x=0;x<240;x++){
      int distanceIndex=y+(x*320);
      int index=distanceIndex*4;
      int intensity=CalculateIntensityFromDistance(depthArray[distanceIndex]);

      colorFrame[index+BlueIndex]=intensity;
      colorFrame[index+GreenIndex]=intensity;
      colorFrame[index+RedIndex]=intensity;
    }
  }

  return colorFrame;
}

Can anyone please help me for this?

Pablovitamin

Hello, Have you found the solution? I am also trying to make it happen. If you have any advances it will be great to know!

lp1394

Hi, for smoothing in VS C++(open CV) I used function : medianblured(image,....)