bool GrayCodePattern_Impl::decode( const std::vector< std::vector<Mat> >& patternImages, OutputArray disparityMap,
InputArrayOfArrays blackImages, InputArrayOfArrays whitheImages, int flags ) const
{
const std::vector<std::vector<Mat> >& acquired_pattern = patternImages;
if( flags == DECODE_3D_UNDERWORLD )
{
// Computing shadows mask
std::vector<Mat> shadowMasks;
computeShadowMasks( blackImages, whitheImages, shadowMasks );
int cam_width = acquired_pattern[0][0].cols;
int cam_height = acquired_pattern[0][0].rows;
Point projPixel;
// Storage for the pixels of the two cams that correspond to the same pixel of the projector
std::vector<std::vector<std::vector<Point> > > camsPixels;
camsPixels.resize( acquired_pattern.size() );
// TODO: parallelize for (k and j)
for( size_t k = 0; k < acquired_pattern.size(); k++ )
{
camsPixels[k].resize( params.height * params.width );
for( int i = 0; i < cam_width; i++ )
{
for( int j = 0; j < cam_height; j++ )
{
//if the pixel is not shadowed, reconstruct
if( shadowMasks[k].at<uchar>( j, i ) )
{
//for a (x,y) pixel of the camera returns the corresponding projector pixel by calculating the decimal number
bool error = getProjPixel( acquired_pattern[k], i, j, projPixel );
if( error )
{
continue;
}
camsPixels[k][projPixel.x * params.height + projPixel.y].push_back( Point( i, j ) );
}
}
}
}
std::vector<Point> cam1Pixs, cam2Pixs;
Mat& disparityMap_ = *( Mat* ) disparityMap.getObj();
disparityMap_ = Mat( cam_height, cam_width, CV_64F, double( 0 ) );
double number_of_pixels_cam1 = 0;
double number_of_pixels_cam2 = 0;
for( int i = 0; i < params.width; i++ )
{
for( int j = 0; j < params.height; j++ )
{
cam1Pixs = camsPixels[0][i * params.height + j];
cam2Pixs = camsPixels[1][i * params.height + j];
if( cam1Pixs.size() == 0 || cam2Pixs.size() == 0 )
continue;
Point p1;
Point p2;
double sump1x = 0;
double sump2x = 0;
number_of_pixels_cam1 += cam1Pixs.size();
number_of_pixels_cam2 += cam2Pixs.size();
for( int c1 = 0; c1 < (int) cam1Pixs.size(); c1++ )
{
p1 = cam1Pixs[c1];
sump1x += p1.x;
}
for( int c2 = 0; c2 < (int) cam2Pixs.size(); c2++ )
{
p2 = cam2Pixs[c2];
sump2x += p2.x;
}
sump2x /= cam2Pixs.size();
sump1x /= cam1Pixs.size();
for( int c1 = 0; c1 < (int) cam1Pixs.size(); c1++ )
{
p1 = cam1Pixs[c1];
disparityMap_.at<double>( p1.y, p1.x ) = ( double ) (sump2x - sump1x);
}
sump2x = 0;
sump1x = 0;
}
}
return true;
} // end if flags
return false;
}
void convexHull( InputArray _points, OutputArray _hull, bool clockwise, bool returnPoints )
{
CV_INSTRUMENT_REGION()
CV_Assert(_points.getObj() != _hull.getObj());
Mat points = _points.getMat();
int i, total = points.checkVector(2), depth = points.depth(), nout = 0;
int miny_ind = 0, maxy_ind = 0;
CV_Assert(total >= 0 && (depth == CV_32F || depth == CV_32S));
if( total == 0 )
{
_hull.release();
return;
}
returnPoints = !_hull.fixedType() ? returnPoints : _hull.type() != CV_32S;
bool is_float = depth == CV_32F;
AutoBuffer<Point*> _pointer(total);
AutoBuffer<int> _stack(total + 2), _hullbuf(total);
Point** pointer = _pointer;
Point2f** pointerf = (Point2f**)pointer;
Point* data0 = points.ptr<Point>();
int* stack = _stack;
int* hullbuf = _hullbuf;
CV_Assert(points.isContinuous());
for( i = 0; i < total; i++ )
pointer[i] = &data0[i];
// sort the point set by x-coordinate, find min and max y
if( !is_float )
{
std::sort(pointer, pointer + total, CHullCmpPoints<int>());
for( i = 1; i < total; i++ )
{
int y = pointer[i]->y;
if( pointer[miny_ind]->y > y )
miny_ind = i;
if( pointer[maxy_ind]->y < y )
maxy_ind = i;
}
}
else
{
std::sort(pointerf, pointerf + total, CHullCmpPoints<float>());
for( i = 1; i < total; i++ )
{
float y = pointerf[i]->y;
if( pointerf[miny_ind]->y > y )
miny_ind = i;
if( pointerf[maxy_ind]->y < y )
maxy_ind = i;
}
}
if( pointer[0]->x == pointer[total-1]->x &&
pointer[0]->y == pointer[total-1]->y )
{
hullbuf[nout++] = 0;
}
else
{
// upper half
int *tl_stack = stack;
int tl_count = !is_float ?
Sklansky_( pointer, 0, maxy_ind, tl_stack, -1, 1) :
Sklansky_( pointerf, 0, maxy_ind, tl_stack, -1, 1);
int *tr_stack = stack + tl_count;
int tr_count = !is_float ?
Sklansky_( pointer, total-1, maxy_ind, tr_stack, -1, -1) :
Sklansky_( pointerf, total-1, maxy_ind, tr_stack, -1, -1);
// gather upper part of convex hull to output
if( !clockwise )
{
std::swap( tl_stack, tr_stack );
std::swap( tl_count, tr_count );
}
for( i = 0; i < tl_count-1; i++ )
hullbuf[nout++] = int(pointer[tl_stack[i]] - data0);
for( i = tr_count - 1; i > 0; i-- )
hullbuf[nout++] = int(pointer[tr_stack[i]] - data0);
int stop_idx = tr_count > 2 ? tr_stack[1] : tl_count > 2 ? tl_stack[tl_count - 2] : -1;
// lower half
int *bl_stack = stack;
int bl_count = !is_float ?
Sklansky_( pointer, 0, miny_ind, bl_stack, 1, -1) :
Sklansky_( pointerf, 0, miny_ind, bl_stack, 1, -1);
int *br_stack = stack + bl_count;
int br_count = !is_float ?
Sklansky_( pointer, total-1, miny_ind, br_stack, 1, 1) :
Sklansky_( pointerf, total-1, miny_ind, br_stack, 1, 1);
if( clockwise )
{
std::swap( bl_stack, br_stack );
std::swap( bl_count, br_count );
}
if( stop_idx >= 0 )
{
int check_idx = bl_count > 2 ? bl_stack[1] :
bl_count + br_count > 2 ? br_stack[2-bl_count] : -1;
if( check_idx == stop_idx || (check_idx >= 0 &&
pointer[check_idx]->x == pointer[stop_idx]->x &&
pointer[check_idx]->y == pointer[stop_idx]->y) )
{
// if all the points lie on the same line, then
// the bottom part of the convex hull is the mirrored top part
// (except the exteme points).
bl_count = MIN( bl_count, 2 );
br_count = MIN( br_count, 2 );
}
}
for( i = 0; i < bl_count-1; i++ )
hullbuf[nout++] = int(pointer[bl_stack[i]] - data0);
for( i = br_count-1; i > 0; i-- )
hullbuf[nout++] = int(pointer[br_stack[i]] - data0);
}
if( !returnPoints )
Mat(nout, 1, CV_32S, hullbuf).copyTo(_hull);
else
{
_hull.create(nout, 1, CV_MAKETYPE(depth, 2));
Mat hull = _hull.getMat();
size_t step = !hull.isContinuous() ? hull.step[0] : sizeof(Point);
for( i = 0; i < nout; i++ )
*(Point*)(hull.ptr() + i*step) = data0[hullbuf[i]];
}
}
