bool csBox3::AdjacentZ (const csBox3 &other, float epsilon) const
{
if (
ABS (other.MinZ () - MaxZ ()) < epsilon ||
ABS (other.MaxZ () - MinZ ()) < epsilon)
{
if (MaxX () < other.MinX () || MinX () > other.MaxX ()) return false;
if (MaxY () < other.MinY () || MinY () > other.MaxY ()) return false;
return true;
}
return false;
}
csVector2 csBox2::GetCorner (int corner) const
{
switch (corner)
{
case CS_BOX_CORNER_xy:
return Min ();
case CS_BOX_CORNER_xY:
return csVector2 (MinX (), MaxY ());
case CS_BOX_CORNER_Xy:
return csVector2 (MaxX (), MinY ());
case CS_BOX_CORNER_XY:
return Max ();
case CS_BOX_CENTER2:
return GetCenter ();
}
return csVector2 (0, 0);
}
int csBox3::Adjacent (const csBox3& other) const
{
if (AdjacentX (other))
{
if (other.MaxX () > MaxX ()) return BOX_SIDE_X;
else return BOX_SIDE_x;
}
if (AdjacentY (other))
{
if (other.MaxY () > MaxY ()) return BOX_SIDE_Y;
else return BOX_SIDE_y;
}
if (AdjacentZ (other))
{
if (other.MaxZ () > MaxZ ()) return BOX_SIDE_Z;
else return BOX_SIDE_z;
}
return -1;
}
double LookupTable::Lookup(double x)
{
if (x < MinX()) return m_below_min;
if (x > MaxX()) return m_above_max;
double x1,y1,x2,y2;
for (unsigned int i = 0; i < m_yvals.size() - 1; ++i) {
if (m_yvals[i+1].first > x) {
x1 = m_yvals[i].first;
x2 = m_yvals[i+1].first;
y1 = m_yvals[i].second;
y2 = m_yvals[i+1].second;
break;
}
}
// Linearly interpolate between the two values
double y = (y2 - y1) * ((x - x1) / (x2 - x1)) + y1;
return y;
//TODO: Neural Network
}
csString csBox2::Description () const
{
csString s;
s.Format ("(%g,%g)-(%g,%g)", MinX (), MinY (), MaxX (), MaxY ());
return s;
}
double tSceneExtents::MaxGeographicWidth() const
{
return MaxX() - MinX();
}
tGeoRect tSceneExtents::BoundingRect() const
{
return tGeoRect(tVector2l(static_cast<long>(floor(MinX())), static_cast<long>(floor(MinY()))),
tVector2l(static_cast<long>(ceil(MaxX())), static_cast<long>(ceil(MaxY()))));
}
void TrackerCvBlobsLib::trackBlobs(const Mat &mat, const Mat &areaMask, bool history, std::vector<Area> *pareas)
{
double min_area = *m_pmin_area;
double max_area = *m_pmax_area;
double max_radius_2 = *m_pmax_radius * *m_pmax_radius;
double x, y, min_x, min_y, max_x, max_y;
double min_depth,max_depth;
Scalar sdepth, sstddev;
cBlob temp;
bool new_hand(true);
int mat_area(mat.size().width*mat.size().height);
// we will convert the matrix object passed from our cFilter class to an object of type IplImage for calling the CBlobResult constructor
IplImage img;
IplImage areaImg;
// storage of the current blobs and the blobs from the previous frame
Size s; Point p;
mat.locateROI(s,p);
areaImg = areaMask;
// convert our OpenCV matrix object to one of type IplImage
img = mat;
// cvblobslib blob extraction
blob_result = CBlobResult(&img, NULL, 1/*img∈{0,255}->thresh unimportant*/, false);
blob_result.Filter(blob_result, B_EXCLUDE, CBlobGetArea(), B_LESS, min_area);
blob_result.Filter(blob_result, B_EXCLUDE, CBlobGetArea(), B_GREATER, max_area);
// clear the blobs from two frames ago
blobs_previous.clear();
// before we populate the blobs vector with the current frame, we need to store the live blobs in blobs_previous
for (int i = 0; i < blobs.size(); i++)
if (blobs[i].event != BLOB_UP)
blobs_previous.push_back(blobs[i]);
// populate the blobs vector with the current frame
blobs.clear();
for (int i = 0; i < blob_result.GetNumBlobs(); i++) {
current_blob = blob_result.GetBlob(i);
x = XCenter(current_blob)/*+m_pSettingKinect->m_kinectProp.roi.x*/;
y = YCenter(current_blob)/*+m_pSettingKinect->m_kinectProp.roi.y*/;
// temp.areaid = areaMask.at<uchar>((int)x+p.x,(int)y+p.y);//?!not works
temp.areaid = (uchar) areaImg.imageData[ ((int)x+p.x) + ((int)y+p.y)*areaMask.size().width];//works
if( temp.areaid == 0 ) continue;
min_x = MinX(current_blob);
min_y = MinY(current_blob);
max_x = MaxX(current_blob);
max_y = MaxY(current_blob);
if( (max_x-min_x)*(max_y-min_y) > 0.9*mat_area) continue;// fix blob detection issue?!
temp.location.x = temp.origin.x = x;
temp.location.y = temp.origin.y = y;
temp.min.x = min_x; temp.min.y = min_y;
temp.max.x = max_x; temp.max.y = max_y;
//Rect r(min_x+p.x,min_y+p.x, max_x-min_y, max_y-min_y);
//Rect r(min_x,min_y, max_x-min_x, max_y-min_y);//width, height +1?!
Rect r( x-3, y-3, min(7,max_x-min_x), min(7, max_y-min_y));
//z = mean( mat(r), mat(r) )[0];/* mean is not good. The blob can include many pixel behind the frame depth*/
/* Depth detection. The measurement method is flexible. */
if( m_pSettingKinect->m_kinectProp.areaThresh ){
/* Mean is ok, because all pixels of the blob are in front of the frame. */
max_depth = mean( mat(r), mat(r) )[0]+4;/*correct blur(1) and area thresh shift (3)*/
//meanStdDev( mat(r), sdepth, sstddev, mat(r) );
//max_depth = sdepth[0]+3*sstddev[0];
//minMaxLoc( mat(r), &min_depth, &max_depth, NULL, NULL, mat(r) );
}else if( pareas != NULL){
/* Remove values behind the area depth and count mean of rest.
This is problematic/choppy if to many pixels are removed.
*/
max_depth = max( (*pareas)[temp.areaid-1].depth-22,
mean( mat(r), mat(r)>(*pareas)[temp.areaid-1].depth-2 )[0] + 1);
}else{
/* Very few information. Use maximum of blob. (Choppy).
* Can be improved, if mean of i.e. 10 biggest values is used
* minMaxLoc require filtered/blured images.
* */
//max_depth = 0;
minMaxLoc( mat(r), &min_depth, &max_depth, NULL, NULL, mat(r) );
}
//printf("Compared depth of area/blob: %i %f\n",(*pareas)[temp.areaid-1].depth ,max_depth);
/* Compare depth of hand with depth of area and throw blob away if hand to far away. */
if(pareas != NULL && max_depth - (*pareas)[temp.areaid-1].depth < -1 ){
//printf("Hand not reached area depth.\n");
continue ;
}
temp.location.z = temp.origin.z = max_depth;
blobs.push_back(temp);
}
// initialize previous blobs to untracked
float d1,d2;
for (int i = 0; i < blobs_previous.size(); i++) blobs_previous[i].tracked = false;
// main tracking loop -- O(n^2) -- simply looks for a blob in the previous frame within a specified radius
for (int i = 0; i < blobs.size(); i++) {
cBlob &blobi = blobs[i];
new_hand = true;
for (int j = 0; j < blobs_previous.size(); j++) {
if (blobs_previous[j].tracked) continue;
d1=blobs[i].location.x - blobs_previous[j].location.x;
d2=blobs[i].location.y - blobs_previous[j].location.y;
if (blobs[i].areaid == blobs_previous[j].areaid
&& (d1*d1 + d2*d2) < max_radius_2) {
blobs_previous[j].tracked = true;
blobs[i].event = BLOB_MOVE;
blobs[i].origin.x = history ? blobs_previous[j].origin.x : blobs_previous[j].location.x;
blobs[i].origin.y = history ? blobs_previous[j].origin.y : blobs_previous[j].location.y;
blobs[i].origin.z = history ? blobs_previous[j].origin.z : blobs_previous[j].location.z;
blobs[i].handid = blobs_previous[j].handid;
blobs[i].cursor = blobs_previous[j].cursor;
blobs[i].cursor25D = blobs_previous[j].cursor25D;
new_hand = false;
break;
}
}
/* assing free handid if new blob */
if( new_hand){
//search next free id.
int next_handid = (last_handid+1) % MAXHANDS;//or = 0;
while( handids[next_handid]==true && next_handid!=last_handid ){
next_handid = (next_handid+1) % MAXHANDS;
} //if array full -> next_handid = last_handid
blobs[i].event = BLOB_DOWN;
blobs[i].handid = next_handid;
blobs[i].cursor = NULL;
blobs[i].cursor25D = NULL;
handids[next_handid] = true;
last_handid = next_handid;
}
}
// add any blobs from the previous frame that weren't tracked as having been removed
for (int i = 0; i < blobs_previous.size(); i++) {
if (!blobs_previous[i].tracked) {
//free handid
handids[blobs_previous[i].handid] = false;
blobs_previous[i].event = BLOB_UP;
blobs.push_back(blobs_previous[i]);
}
}
/*
for (int i = 1; i < blob_result.GetNumBlobs(); i++) {
current_blob = blob_result.GetBlob(i);
printf("Blobcoordsd %f, %f\n", XCenter(current_blob), YCenter(current_blob) );
}
*/
int counter = 0;
cBlob tb;
for (int i = 0; i < blobs.size(); i++) {
if( blobs[i].event != BLOB_UP ){
counter++;
tb = blobs[i];
//printf("Blobcoordsd %f, %f\n", blobs[i].location.x, blobs[i].location.y );
//printf("Blob areaid: %i, handid: %i, (%f,%f)\n", blobs[i].areaid, blobs[i].handid, blobs[i].location.x, blobs[i].location.y );
if(! *m_pnotDrawBlob ){
cvLine(&img,
Point((int)tb.origin.x,(int)tb.origin.y),
Point((int)tb.location.x,(int)tb.location.y),Scalar(244),2);
cvRectangle(&img,
Point((int)tb.min.x,(int)tb.min.y),
Point((int)tb.max.x,(int)tb.max.y),Scalar(255),2);
}
}
}
// printf("Active blobs: %i %i %i\n",counter, blobs.size(), blob_result.GetNumBlobs());
/* for(int i=0; i<MAXHANDS; i++){
printf("%i,", (handids[i]==true)?1:0);
}
printf("\n");*/
}
//****************************************************************************
//
// * Get the value at x, y
//============================================================================
SJCVector2d SJCVectorField2d::
Value(const double x_pos, const double y_pos)
//============================================================================
{
// Check whether the position is out of bound
if(x_pos < MinX() || x_pos > MaxX() ||
y_pos < MinY() || y_pos > MaxY()){
SJCWarning("X or Y out of bound in getting value");
return SJCVector2d(0.f, 0.f);
}
// Compute the index position
double x = (x_pos - m_dHalfDX) / m_dDX;
double y = (y_pos - m_dHalfDY) / m_dDY;
uint index_xl, index_xr; // the left and right index in x
uint index_yb, index_yt; // The top and bottom index in y
double partial_x, partial_y; // The partial in x and y
// Here should have judgement when x_pos and y_pos are close to index
// position
switch ( m_VBoundary[0] ) { // Check and set up the correct value for x,y
// according the boundary conditions
case BOUNDARY_NOWRAP:
case BOUNDARY_NOWRAP_FREE:
if ( x == m_uNX - 1.f) {
index_xr = (uint)m_uNX - 1;
index_xl = index_xr - 1;
partial_x = 1.0f;
}
else {
index_xl = (uint)floor(x);
index_xr = index_xl + 1;
partial_x = x - (double)index_xl;
}
break;
case BOUNDARY_WRAP:
double xp = fmod(x, (double)m_uNX);
if ( xp < 0.0 )
xp = xp + (int)m_uNX;
index_xl = (uint)floor(xp);
index_xr = (index_xl + 1) % m_uNX;
partial_x = xp - (double)index_xl;
break;
} // end of switch
switch ( m_VBoundary[1] ) {
case BOUNDARY_NOWRAP:
case BOUNDARY_NOWRAP_FREE:
if ( y == m_uNY - 1.f ) {
index_yt = (uint)m_uNY - 1;
index_yb = index_yt - 1;
partial_y = 1.0f;
}
else {
index_yb = (uint)floor(y);
index_yt = index_yb + 1;
partial_y = y - (double)index_yb;
}
break;
case BOUNDARY_WRAP:
double yp = fmod(y, (double)m_uNY);
if ( yp < 0.0 )
yp = yp + (int)m_uNY;
index_yb = (uint)floor(yp);
index_yt = (index_yb + 1) % m_uNY;
partial_y = yp - (double)index_yb;
break;
}
SJCVector2d vBottomLeft = m_VData[Index(index_xl, index_yb)];
SJCVector2d vTopLeft = m_VData[Index(index_xl, index_yt)];
SJCVector2d vBottomRight = m_VData[Index(index_xr, index_yb)];
SJCVector2d vTopRight = m_VData[Index(index_xr, index_yt)];
return (1.f-partial_x)*(1.f-partial_y) * vBottomLeft +
partial_y *(1.f - partial_x) * vTopLeft +
partial_x *(1.f - partial_y) * vBottomRight +
partial_x *partial_y * vTopRight;
}