mirror of
https://github.com/storytold/storyteller-ml.git
synced 2026-10-18 07:41:03 +00:00
Kasisnu
434 lines
20 KiB
C++
434 lines
20 KiB
C++
#include "cameraControl.hpp"
|
|
|
|
#include "../MocapNET2/MocapNETLib2/IO/commonSkeleton.hpp"
|
|
|
|
#include <cstdlib>
|
|
|
|
|
|
const unsigned int noiseThreshold = 30; //pixels
|
|
|
|
|
|
/**
|
|
* @brief In order to have the best possible quality we can crop the input frame to only perform detection around the area of the previous skeleton
|
|
* This code performs this crop and tries to get the best detection window
|
|
* @ingroup demo
|
|
* @bug This code is oriented to a single 2D skeleton detected, Multiple skeletons will confuse it and there is no logic to handle them
|
|
* @retval 1=Success/0=Failure
|
|
*/
|
|
int getMaximumCropWindow(
|
|
unsigned int * x,
|
|
unsigned int * y,
|
|
unsigned int * width,
|
|
unsigned int * height,
|
|
struct boundingBox * bboxExternal,
|
|
unsigned int inputWidth2DJointDetector,
|
|
unsigned int inputHeight2DJointDetector,
|
|
unsigned int fullFrameWidth,
|
|
unsigned int fullFrameHeight
|
|
)
|
|
{
|
|
//Make a local copy in case of failure
|
|
struct boundingBox bboxLocal = *bboxExternal;
|
|
struct boundingBox * bbox = &bboxLocal;
|
|
|
|
|
|
float paddingPercent=0.2; // 0.1 = 10% etc
|
|
|
|
if ( (bbox!=0) && (bbox->populated) )
|
|
{
|
|
unsigned int previousX = *x;
|
|
unsigned int previousY = *y;
|
|
unsigned int previousWidth = *width;
|
|
unsigned int previousHeight = *height;
|
|
|
|
//std::cerr<<"Previous Bounding Box was "<<*x<<","<<*y<<" to "<<*x+*width<<","<<*y+*height<<" \n";
|
|
|
|
// Bring bounding box to normal coordinates
|
|
//------------------------------------------------
|
|
bbox->minimumX+= (float) *x;
|
|
bbox->maximumX+= (float) *x;
|
|
bbox->minimumY+= (float) *y;
|
|
bbox->maximumY+= (float) *y;
|
|
|
|
//Calculate the dimensions of the bounding box body and its center
|
|
//----------------------------------------------------------------------------------------------------------
|
|
float bodyWidth = bbox->maximumX - bbox->minimumX;
|
|
float bodyHeight = bbox->maximumY - bbox->minimumY;
|
|
float bodyCenterX = bbox->minimumX + (float) bodyWidth/2;
|
|
float bodyCenterY = bbox->minimumY + (float) bodyHeight/2;
|
|
|
|
|
|
//If the body is smaller than our input size lets get some more detail in so we avoid scaling up
|
|
//----------------------------------------------------------------------------------------------------------------------------------------------------
|
|
if (inputWidth2DJointDetector >bodyWidth )
|
|
{
|
|
bodyWidth = inputWidth2DJointDetector;
|
|
}
|
|
if (inputHeight2DJointDetector> bodyHeight )
|
|
{
|
|
bodyHeight = inputHeight2DJointDetector;
|
|
}
|
|
|
|
|
|
//Given that we have our new size and center we can now calculate the new offset from center
|
|
//--------------------------------------------------------------------------------------------------------------------------------------------------------
|
|
float bodyOffsetX = (float) bodyWidth/2;
|
|
float bodyOffsetY = (float) bodyHeight/2;
|
|
|
|
//And using the new offset derive the new bounding box
|
|
//--------------------------------------------------------------------------------------------------------------------------------------------------------
|
|
float pixelsPaddedX = paddingPercent * bodyWidth;
|
|
float pixelsPaddedY = paddingPercent * bodyHeight;
|
|
|
|
//We now have a preliminary bounding box BUT the final bounding box we want is a rectangle so we need to keep the biggest dimension
|
|
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
|
|
if ( bodyOffsetX+pixelsPaddedX> bodyOffsetY+pixelsPaddedY)
|
|
{
|
|
bodyOffsetX = bodyOffsetX + pixelsPaddedX;
|
|
bodyOffsetY = bodyOffsetX + pixelsPaddedX;
|
|
}
|
|
else
|
|
{
|
|
bodyOffsetX = bodyOffsetY + pixelsPaddedY;
|
|
bodyOffsetY = bodyOffsetY + pixelsPaddedY;
|
|
}
|
|
|
|
//We now have a rectangle bounding box
|
|
//----------------------------------------------------------------
|
|
bbox->minimumX = bodyCenterX - bodyOffsetX;
|
|
bbox->maximumX = bodyCenterX + bodyOffsetX;
|
|
bbox->minimumY = bodyCenterY - bodyOffsetY;
|
|
bbox->maximumY = bodyCenterY + bodyOffsetY;
|
|
|
|
//We do a last bounds check
|
|
//--------------------------------------------------------------------------------------------------------------------------------------------------------
|
|
signed int limitLX=0,limitLY=0,limitRX=0,limitRY=0;
|
|
if (bbox->minimumX < 0 )
|
|
{
|
|
limitLX=-1*bbox->minimumX;
|
|
bbox->minimumX = 0;
|
|
}
|
|
if (bbox->minimumY < 0 )
|
|
{
|
|
limitLY=-1*bbox->minimumY;
|
|
bbox->minimumY = 0;
|
|
}
|
|
if (bbox->maximumX > fullFrameWidth )
|
|
{
|
|
limitRX=bbox->maximumX-fullFrameWidth;
|
|
bbox->maximumX = fullFrameWidth;
|
|
}
|
|
if (bbox->maximumY > fullFrameHeight )
|
|
{
|
|
limitRY=bbox->maximumY-fullFrameHeight;
|
|
bbox->maximumY = fullFrameHeight;
|
|
}
|
|
|
|
|
|
//We now have a preliminary bounding box the rectangle might be in a corner and fall out of bounds
|
|
//--------------------------------------------------------------------------------------------------------------------------------------------------------
|
|
|
|
//We are maybe constrained left/up add what we lost right/down..!
|
|
if (limitLX>0)
|
|
{
|
|
bbox->maximumX += limitLX;
|
|
}
|
|
if (limitLY>0)
|
|
{
|
|
bbox->maximumY += limitLY;
|
|
}
|
|
|
|
//We are maybe constrained right/down we subtract what we lost left/up ..!
|
|
if (limitRX>0)
|
|
{
|
|
bbox->minimumX -= limitRX;
|
|
}
|
|
if (limitRY>0)
|
|
{
|
|
bbox->minimumY -= limitRY;
|
|
}
|
|
|
|
if ( (limitLX!=0) || (limitLY!=0) || (limitRX!=0) || (limitRY!=0) )
|
|
{
|
|
//We have failed to initially resolve the bounding box..!
|
|
//std::cerr<<"Initial resolve the bounding box X1:"<<limitLX<<",Y1:"<<limitLY<<",X2:"<<limitRX<<",Y2:"<<limitRY<<"..\n";
|
|
//return 0;
|
|
}
|
|
|
|
|
|
|
|
//We one last time check for bound limits
|
|
limitLX=0,limitLY=0,limitRX=0,limitRY=0;
|
|
if (bbox->minimumX < 0 )
|
|
{
|
|
limitLX=1;
|
|
bbox->minimumX = 0;
|
|
}
|
|
if (bbox->minimumY < 0 )
|
|
{
|
|
limitLY=1;
|
|
bbox->minimumY = 0;
|
|
}
|
|
if (bbox->maximumX > fullFrameWidth )
|
|
{
|
|
limitRX=1;
|
|
bbox->maximumX = fullFrameWidth;
|
|
}
|
|
if (bbox->maximumY > fullFrameHeight )
|
|
{
|
|
limitRY=1;
|
|
bbox->maximumY = fullFrameHeight;
|
|
}
|
|
|
|
if ( (limitLX!=0) || (limitLY!=0) || (limitRX!=0) || (limitRY!=0) )
|
|
{
|
|
//We have failed to resolve the bounding box..!
|
|
//std::cerr<<"We have failed to resolve the bounding box X1:"<<limitLX<<",Y1:"<<limitLY<<",X2:"<<limitRX<<",Y2:"<<limitRY<<"..\n";
|
|
//std::cerr<<"bounding box was X1:"<<bbox->minimumX<<",Y1:"<<bbox->minimumY<<",X2:"<<bbox->maximumX<<",Y2:"<<bbox->maximumY<<"..\n";
|
|
return 0;
|
|
}
|
|
|
|
|
|
//And our output is ready for use..!
|
|
//--------------------------------------------------------------------------------------------------------------------------------------------------------
|
|
if (
|
|
( abs(previousX - (unsigned int) bbox->minimumX ) > noiseThreshold ) ||
|
|
( abs(previousY - (unsigned int) bbox->minimumY ) > noiseThreshold ) ||
|
|
( abs(previousWidth - (unsigned int) ( bbox->maximumX - bbox->minimumX ) ) > 2*noiseThreshold ) ||
|
|
( abs(previousHeight - (unsigned int) ( bbox->maximumY - bbox->minimumY ) ) > 2*noiseThreshold )
|
|
)
|
|
{
|
|
*x=(unsigned int) bbox->minimumX ;
|
|
*y=(unsigned int) bbox->minimumY ;
|
|
*width = (unsigned int) bbox->maximumX - bbox->minimumX;
|
|
*height= (unsigned int) bbox->maximumY - bbox->minimumY;
|
|
|
|
if (*width == *height +1 )
|
|
{
|
|
*width = *height;
|
|
}
|
|
|
|
//---------------------------------------------------------------------------------------------------------------------------------------------------------------
|
|
//std::cerr<<"New MAX Bounding Box is "<<*x<<","<<*y<<" to "<<*x+*width<<","<<*y+*height<<" Image("<<fullFrameWidth<<","<<fullFrameHeight<<")\n";
|
|
|
|
if ( (*width!=0) && (*height!=0) )
|
|
{
|
|
//Success..!
|
|
*bboxExternal=bboxLocal;
|
|
return 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
fprintf(stderr,"Bounding box is too similar with previous so keeping previous..\n");
|
|
//std::cerr<<"New MAX Bounding Box would have been "<< bbox->minimumX<<","<< bbox->minimumY<<" to "<< bbox->maximumX<<","<< bbox->maximumY<<" Image("<<fullFrameWidth<<","<<fullFrameHeight<<")\n";
|
|
}
|
|
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief In order to have the best possible quality we can crop the input frame to only perform detection around the area of the previous skeleton
|
|
* This code performs this crop and tries to get the best detection window
|
|
* @ingroup demo
|
|
* @bug This code is oriented to a single 2D skeleton detected, Multiple skeletons will confuse it and there is no logic to handle them
|
|
* @retval 1=Success/0=Failure
|
|
*/
|
|
int getBestCropWindow(
|
|
int maximumCrop,
|
|
unsigned int * x,
|
|
unsigned int * y,
|
|
unsigned int * width,
|
|
unsigned int * height,
|
|
struct boundingBox * bbox,
|
|
unsigned int inputWidth2DJointDetector,
|
|
unsigned int inputHeight2DJointDetector,
|
|
unsigned int fullFrameWidth,
|
|
unsigned int fullFrameHeight
|
|
)
|
|
{
|
|
//fprintf(stderr,"getBestCropWindow(%u,%u,%u,%u)\n",*x,*y,*width,*height);
|
|
//fprintf(stderr,"input image size is %u,%u\n",fullFrameWidth,fullFrameHeight);
|
|
if (bbox==0) { return 0; }
|
|
|
|
if (maximumCrop)
|
|
{
|
|
if ( getMaximumCropWindow(
|
|
x,
|
|
y,
|
|
width,
|
|
height,
|
|
bbox,
|
|
inputWidth2DJointDetector,
|
|
inputHeight2DJointDetector,
|
|
fullFrameWidth,
|
|
fullFrameHeight
|
|
)
|
|
)
|
|
{
|
|
return 1;
|
|
}
|
|
//If we fail to get Maximum Crop Window we will settle with normal crop window
|
|
}
|
|
|
|
//fprintf(stderr,"Previous crop started at %u,%u\n", *x,*y);
|
|
if ( (bbox!=0) && (bbox->populated) )
|
|
{
|
|
//fprintf(stderr,"This means that the bounding box (%0.2f,%0.2f) -> (%0.2f,%0.2f)\n",bbox->minimumX,bbox->minimumY,bbox->maximumX,bbox->maximumY);
|
|
bbox->minimumX+=(float) *x;
|
|
bbox->maximumX+=(float) *x;
|
|
bbox->minimumY+=(float) *y;
|
|
bbox->maximumY+=(float) *y;
|
|
//fprintf(stderr,"is actually (%0.2f,%0.2f) -> (%0.2f,%0.2f)\n",bbox->minimumX,bbox->minimumY,bbox->maximumX,bbox->maximumY);
|
|
}
|
|
|
|
|
|
unsigned int dimension = fullFrameHeight;
|
|
float bodyWidth = bbox->maximumX - bbox->minimumX;
|
|
float bodyHeight = bbox->maximumY - bbox->minimumY;
|
|
//fprintf(stderr,"Body starts at %0.2f and ends at %0.2f for a total of %0.2f pixels\n", bbox->minimumX , bbox->maximumX , bbox->maximumX-bbox->minimumX);
|
|
if (fullFrameWidth>=fullFrameHeight)
|
|
{
|
|
//fprintf(stderr,"Since the whole frame has %u pixels\n",fullFrameWidth);
|
|
//fprintf(stderr,"And we will use the dimension of the Y axis aka %u pixels to crop a rectangle\n",fullFrameHeight);
|
|
|
|
if ( (bbox!=0) && (bbox->populated) )
|
|
{
|
|
dimension = fullFrameHeight;
|
|
//TODO:
|
|
//if (bodyHeight<dimension) { dimension = bodyHeight; }
|
|
//if (dimension<inputHeight2DJointDetector) { dimension = inputHeight2DJointDetector; }
|
|
|
|
*width=dimension;
|
|
*height=dimension;
|
|
|
|
//-------------------------------------------------------------------------------------------------------
|
|
//-------------------------------------------------------------------------------------------------------
|
|
// Center on X axis..
|
|
//-------------------------------------------------------------------------------------------------------
|
|
//-------------------------------------------------------------------------------------------------------
|
|
float bodyCenterX = bbox->minimumX + (float) bodyWidth/2;
|
|
//fprintf(stderr,"The center X of the body lies at %0.2f\n",bodyCenterX);
|
|
|
|
//fprintf(stderr,"We can afford %u pixels left and right of the body center \n",(unsigned int) *width/2);
|
|
float cropStartX = bodyCenterX - *width/2;
|
|
|
|
if (cropStartX<0)
|
|
{
|
|
cropStartX=0; //Overflow..
|
|
}
|
|
if (cropStartX>fullFrameWidth-fullFrameHeight)
|
|
{
|
|
cropStartX=fullFrameWidth-fullFrameHeight;
|
|
}
|
|
|
|
//Neural Networks cause flicker, if we don't have exactly the same
|
|
//bounding box it's ok just keep the previous, it will help with flicker a lot..
|
|
if ( abs(*x - (unsigned int) cropStartX) > noiseThreshold )
|
|
{
|
|
*x = (unsigned int) cropStartX;
|
|
}
|
|
//fprintf(stderr,"This means starting at %u and ending at %u\n",(unsigned int) *x, *x+*width);
|
|
|
|
/*
|
|
//-------------------------------------------------------------------------------------------------------
|
|
//-------------------------------------------------------------------------------------------------------
|
|
// Center on Y axis..
|
|
//-------------------------------------------------------------------------------------------------------
|
|
//-------------------------------------------------------------------------------------------------------
|
|
float bodyCenterY = bbox->minimumY+ (float) bodyHeight/2;
|
|
//fprintf(stderr,"The center X of the body lies at %0.2f\n",bodyCenterX);
|
|
|
|
//fprintf(stderr,"We can afford %u pixels up and down of the body center \n",(unsigned int) *height/2);
|
|
float cropStartY = bodyCenterY - *height/2;
|
|
|
|
if (cropStartY<0) { cropStartY=0; } //Overflow..
|
|
if (cropStartY>fullFrameHeight) { cropStartY=fullFrameHeight; }
|
|
|
|
//Neural Networks cause flicker, if we don't have exactly the same
|
|
//bounding box it's ok just keep the previous, it will help with flicker a lot..
|
|
if ( abs(*y - (unsigned int) cropStartY) > 4 )
|
|
{
|
|
*y = (unsigned int) cropStartY;
|
|
}
|
|
*/
|
|
|
|
|
|
//std::cerr<<"Normal Bounding Box derived "<<*x<<","<<*y<<" to "<<*x+*width<<","<<*y+*height<<" \n";
|
|
}
|
|
else
|
|
{
|
|
//No skeleton? just crop in the center..
|
|
*x=(fullFrameWidth-fullFrameHeight)/2;
|
|
*y=0;
|
|
*width=fullFrameHeight;
|
|
*height=fullFrameHeight;
|
|
}
|
|
}
|
|
else if (fullFrameWidth<fullFrameHeight)
|
|
{
|
|
if ( (bbox!=0) && (bbox->populated) )
|
|
{
|
|
dimension = fullFrameWidth;
|
|
//TODO:
|
|
//if (bodyHeight<dimension) { dimension = bodyHeight; }
|
|
//if (dimension<inputHeight2DJointDetector) { dimension = inputHeight2DJointDetector; }
|
|
|
|
*width=dimension;
|
|
*height=dimension;
|
|
|
|
//-------------------------------------------------------------------------------------------------------
|
|
//-------------------------------------------------------------------------------------------------------
|
|
// Center on X axis..
|
|
//-------------------------------------------------------------------------------------------------------
|
|
//-------------------------------------------------------------------------------------------------------
|
|
float bodyCenterY = bbox->minimumY + (float) bodyHeight/2;
|
|
//fprintf(stderr,"The center X of the body lies at %0.2f\n",bodyCenterX);
|
|
|
|
//fprintf(stderr,"We can afford %u pixels left and right of the body center \n",(unsigned int) *width/2);
|
|
float cropStartY = bodyCenterY - *width/2;
|
|
|
|
if (cropStartY<0)
|
|
{
|
|
cropStartY=0; //Overflow..
|
|
}
|
|
if (cropStartY>fullFrameHeight-fullFrameWidth)
|
|
{
|
|
cropStartY=fullFrameHeight-fullFrameWidth;
|
|
}
|
|
|
|
//Neural Networks cause flicker, if we don't have exactly the same
|
|
//bounding box it's ok just keep the previous, it will help with flicker a lot..
|
|
if ( abs(*y - (unsigned int) cropStartY) > noiseThreshold )
|
|
{
|
|
*y = (unsigned int) cropStartY;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//No skeleton? just crop in the center..
|
|
*x=0;
|
|
*y=(fullFrameHeight-fullFrameWidth)/2;
|
|
*width=fullFrameWidth;
|
|
*height=fullFrameWidth;
|
|
}
|
|
}
|
|
|
|
if (
|
|
( *x + *width > fullFrameWidth ) ||
|
|
( *y + *height > fullFrameHeight )
|
|
)
|
|
{
|
|
//We failed..!
|
|
*x=0;
|
|
*y=0;
|
|
*width=fullFrameWidth;
|
|
*height=fullFrameWidth;
|
|
return 0;
|
|
}
|
|
|
|
return 1;
|
|
}
|