bool EdisonSegmModule::updateModule()
{
ImageOf<PixelRgb> *yrpImgIn;
static int cycles = 0;
yrpImgIn = _imgPort.read();
if (yrpImgIn == NULL) // this is the case if module is requested to quit while waiting for image
return true;
bool use_private_stamp;
Stamp s;
if(!_imgPort.getEnvelope(s))
{
cout << "No stamp found in input image. Will use private stamp" << endl;
use_private_stamp = true;
}
else
{
cout << "Received image #" << s.getCount() << " generated at time " << s.getTime() << endl;
use_private_stamp = false;
}
if(cycles == 0)
_timestart = yarp::os::Time::now();
cycles++;
IplImage *iplimg = (IplImage*)yrpImgIn->getIplImage();
//computing the ROI to crop the image
/*struct _IplROI roi;
roi.coi = 0; // all channels are selected
roi.height = height_;
roi.width = width_;
roi.xOffset = ( orig_width_ - width_ ) / 2;
roi.yOffset = ( orig_height_ - height_ ) / 2;*/
//copying roi data to buffer
/*iplimg->roi = &roi;
cvCopy( iplimg, inputImage.getIplImage());*/
//Rescale image if required
if( (width_ != orig_width_) || (height_ != orig_height_ ) )
cvResize(iplimg, inputImage.getIplImage(), CV_INTER_NN);
else
cvCopy( iplimg, inputImage.getIplImage());
double edgetime = yarp::os::Time::now();
//compute gradient and confidence maps
BgEdgeDetect edgeDetector(gradWindRad);
BgImage bgImage;
bgImage.SetImage(inputImage_, width_, height_, true);
edgeDetector.ComputeEdgeInfo(&bgImage, confMap_, gradMap_);
//compute the weigth map
for(int i = 0; i < width_*height_; i++) {
if(gradMap_[i] > 0.02) {
weightMap_[i] = mixture*gradMap_[i] + (1 - mixture)*confMap_[i];
} else {
weightMap_[i] = 0;
}
}
///////////////////////////// This block can be parallelized
cout << "Edge computation Time (ms): " << (yarp::os::Time::now() - edgetime)*1000.0 << endl;
msImageProcessor iProc;
if( dim_ == 3 )
iProc.DefineImage(inputImage_, COLOR, height_, width_);
else
{
cvCvtColor(inputImage.getIplImage(), inputHsv.getIplImage(), CV_RGB2HSV);
cvSplit(inputHsv.getIplImage(), inputHue.getIplImage(), 0, 0, 0);
iProc.DefineImage(inputHue_, GRAYSCALE, height_, width_);
}
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
iProc.SetWeightMap(weightMap_, threshold);
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
double filtertime = yarp::os::Time::now();
iProc.Filter(sigmaS, sigmaR, speedup);
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
cout << "Mean Shift Filter Computation Time (ms): " << (yarp::os::Time::now() - filtertime)*1000.0 << endl;
//obtain the filtered image
iProc.GetResults(filtImage_);
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
//fuse regions
double fusetime = yarp::os::Time::now();
iProc.FuseRegions(sigmaR, minRegion);
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
cout << "Region Fusion Computation Time (ms): " << (yarp::os::Time::now() - fusetime)*1000.0 << endl;
//obtain the segmented image
iProc.GetResults(segmImage_);
if(iProc.ErrorStatus) {
cout << "MeanShift Error" << endl;
return false;
}
//define the boundaries - do not need this
/*
RegionList *regionList = iProc.GetBoundaries();
int *regionIndeces = regionList->GetRegionIndeces(0);
int numRegions = regionList->GetNumRegions();
numBoundaries_ = 0;
for(int i = 0; i < numRegions; i++) {
numBoundaries_ += regionList->GetRegionCount(i);
}
if(boundaries_) delete [] boundaries_;
boundaries_ = new int [numBoundaries_];
for(int i = 0; i < numBoundaries_; i++) {
boundaries_[i] = regionIndeces[i];
}*/
int regionCount; // how many regions have been found
int *labels = NULL; //pointer for the labels (should this be released in the end?)
float *modes; //pointer for the Luv values (should this be released in the end?)
int *modePointCounts; //the area of each region (should this be released in the end?)
regionCount = iProc.GetRegions(&labels, &modes, &modePointCounts);
int *labelp = (int*)labelImage.getRawImage();
for(int i = 0; i < width_*height_; i++)
labelp[i] = labels[i];
IplImage *labelint = (IplImage*)labelImage.getIplImage();
IplImage *labelchar = (IplImage*)labelView.getIplImage();
cvConvert(labelint, labelchar);
//prepare timestamps
if(use_private_stamp)
{
_stamp.update();
_labelPort.setEnvelope(_stamp);
_labelViewPort.setEnvelope(_stamp);
_viewPort.setEnvelope(_stamp);
_filtPort.setEnvelope(_stamp);
_rawPort.setEnvelope(_stamp);
}
else
{
_labelPort.setEnvelope(s);
_labelViewPort.setEnvelope(s);
_viewPort.setEnvelope(s);
_filtPort.setEnvelope(s);
_rawPort.setEnvelope(s);
}
ImageOf<PixelInt> &yrpImgLabel = _labelPort.prepare();
//Rescale image if required
if( (width_ != orig_width_) || (height_ != orig_height_ ) )
{
yrpImgLabel.resize(orig_width_, orig_height_);
cvResize(labelImage.getIplImage(), yrpImgLabel.getIplImage(), CV_INTER_NN);
}
else
yrpImgLabel = labelImage;
_labelPort.write();
ImageOf<PixelMono> &yrpImgDebug = _labelViewPort.prepare();
//Rescale image if required
if( (width_ != orig_width_) || (height_ != orig_height_ ) )
{
yrpImgDebug.resize(orig_width_, orig_height_);
cvResize(labelView.getIplImage(), yrpImgDebug.getIplImage(), CV_INTER_NN);
}
else
yrpImgDebug = labelView;
_labelViewPort.write();
ImageOf<PixelRgb> &yrpFiltOut = _filtPort.prepare();
//Rescale image if required
if( (width_ != orig_width_) || (height_ != orig_height_ ) )
{
yrpFiltOut.resize(orig_width_, orig_height_);
cvResize(filtImage.getIplImage(), yrpFiltOut.getIplImage(), CV_INTER_NN);
}
else
yrpFiltOut = filtImage;
_filtPort.write();
ImageOf<PixelRgb> &yrpImgView = _viewPort.prepare();
//Rescale image if required
if( (width_ != orig_width_) || (height_ != orig_height_ ) )
{
yrpImgView.resize(orig_width_, orig_height_);
cvResize(segmImage.getIplImage(), yrpImgView.getIplImage(), CV_INTER_NN);
}
else
yrpImgView = segmImage;
_viewPort.write();
ImageOf<PixelRgb> &yrpImgOut = _rawPort.prepare();
yrpImgOut = *yrpImgIn;
_rawPort.write();
//report the frame rate
if(cycles % 100 == 0)
{
double cps = ((double)cycles)/(yarp::os::Time::now() - _timestart);
printf("fps: %02.2f\n", cps);
}
return true;
}
/*
* main enterance point
*/
void mexFunction(
int nlhs, /* number of expected outputs */
mxArray *plhs[], /* mxArray output pointer array */
int nrhs, /* number of inputs */
const mxArray *prhs[] /* mxArray input pointer array */
)
{
if ( nrhs != 3 )
mexErrMsgIdAndTxt("edison_wraper:main","Must have three inputs");
if ( mxGetClassID(prhs[0]) != mxSINGLE_CLASS )
mexErrMsgIdAndTxt("edison_wraper:main","fim must be of type single");
if ( mxGetClassID(prhs[1]) != mxUINT8_CLASS )
mexErrMsgIdAndTxt("edison_wraper:main","rgbim must be of type uint8");
if ( mxGetClassID(prhs[2]) != mxSTRUCT_CLASS )
mexErrMsgIdAndTxt("edison_wraper:main","parameters argument must be a structure");
/* first argument - the image in whatever feature space it is given in */
float * fimage = (float*)mxGetData(prhs[0]);
const int* image_dims = mxGetDimensions(prhs[0]);
int image_ndim = mxGetNumberOfDimensions(prhs[0]);
unsigned int w = image_dims[1];
unsigned int h = image_dims[2];
unsigned int N = image_dims[0];
int ii;
unsigned char * rgbim = (unsigned char*)mxGetData(prhs[1]);
const int * rgb_dims = mxGetDimensions(prhs[1]);
if ( rgb_dims[1] != w || rgb_dims[2] != h )
mexErrMsgIdAndTxt("edison_wraper:main","size of fim and rgbim do not match");
/* second input - parameters structure */
// 'steps' - What steps the algorithm should perform:
// 1 - only mean shift filtering
// 2 - filtering and region fusion
// 3 - full segmentation process [default]
int steps;
GetScalar(mxGetField(prhs[2], 0, "steps"), steps);
// 'synergistic' - perform synergistic segmentation [true]|false
bool syn;
GetScalar(mxGetField(prhs[2], 0, "synergistic"), syn);
// 'SpatialBandWidth' - segmentation spatial radius (integer) [7]
unsigned int spBW;
GetScalar(mxGetField(prhs[2], 0, "SpatialBandWidth"), spBW);
// 'RangeBandWidth' - segmentation feature space radius (float) [6.5]
float fsBW;
GetScalar(mxGetField(prhs[2], 0, "RangeBandWidth"), fsBW);
// 'MinimumRegionArea'- minimum segment area (integer) [20]
unsigned int minArea;
GetScalar(mxGetField(prhs[2], 0, "MinimumRegionArea"), minArea);
// 'SpeedUp' - algorithm speed up {0,1,2} [1]
int SpeedUp;
enum SpeedUpLevel sul;
GetScalar(mxGetField(prhs[2], 0, "SpeedUp"), SpeedUp);
switch (SpeedUp) {
case 1:
sul = NO_SPEEDUP;
break;
case 2:
sul = MED_SPEEDUP;
break;
case 3:
sul = HIGH_SPEEDUP;
break;
default:
mexErrMsgIdAndTxt("edison_wraper:main","wrong speedup value");
}
// 'GradientWindowRadius' - synergistic parameters (integer) [2]
unsigned int grWin;
GetScalar(mxGetField(prhs[2], 0, "GradientWindowRadius"), grWin);
// 'MixtureParameter' - synergistic parameter (float 0,1) [.3]
float aij;
GetScalar(mxGetField(prhs[2], 0, "MixtureParameter"), aij);
// 'EdgeStrengthThreshold'- synergistic parameter (float 0,1) [.3]
float edgeThr;
GetScalar(mxGetField(prhs[2], 0, "EdgeStrengthThreshold"), edgeThr);
msImageProcessor ms;
ms.DefineLInput(fimage, h, w, N); // image array should be after permute
if (ms.ErrorStatus)
mexErrMsgIdAndTxt("edison_wraper:edison","Mean shift define latice input: %s", ms.ErrorMessage);
kernelType k[2] = {DefualtKernelType, DefualtKernelType};
int P[2] = {DefualtSpatialDimensionality, N};
float tempH[2] = {1.0, 1.0};
ms.DefineKernel(k, tempH, P, 2);
if (ms.ErrorStatus)
mexErrMsgIdAndTxt("edison_wraper:edison","Mean shift define kernel: %s", ms.ErrorMessage);
mxArray * mxconf = NULL;
float * conf = NULL;
mxArray * mxgrad = NULL;
float * grad = NULL;
mxArray * mxwght = NULL;
float * wght = NULL;
if (syn) {
/* perform synergistic segmentation */
int maps_dim[2] = {w*h, 1};
/* allcate memory for confidence and gradient maps */
mxconf = mxCreateNumericArray(2, maps_dim, mxSINGLE_CLASS, mxREAL);
conf = (float*)mxGetData(mxconf);
mxgrad = mxCreateNumericArray(2, maps_dim, mxSINGLE_CLASS, mxREAL);
grad = (float*)mxGetData(mxgrad);
BgImage rgbIm;
rgbIm.SetImage(rgbim, w, h, rgb_dims[0] == 3);
BgEdgeDetect edgeDetector(grWin);
edgeDetector.ComputeEdgeInfo(&rgbIm, conf, grad);
mxwght = mxCreateNumericArray(2, maps_dim, mxSINGLE_CLASS, mxREAL);
wght = (float*)mxGetData(mxgrad);
for ( ii = 0 ; ii < w*h; ii++ ) {
wght[ii] = (grad[ii] > .002) ? aij*grad[ii]+(1-aij)*conf[ii] : 0;
}
ms.SetWeightMap(wght, edgeThr);
if (ms.ErrorStatus)
mexErrMsgIdAndTxt("edison_wraper:edison","Mean shift set weights: %s", ms.ErrorMessage);
}
ms.Filter(spBW, fsBW, sul);
if (ms.ErrorStatus)
mexErrMsgIdAndTxt("edison_wraper:edison","Mean shift filter: %s", ms.ErrorMessage);
if (steps == 2) {
ms.FuseRegions(fsBW, minArea);
if (ms.ErrorStatus)
mexErrMsgIdAndTxt("edison_wraper:edison","Mean shift fuse: %s", ms.ErrorMessage);
}
if ( nlhs >= 1 ) {
// first output - the feture space raw image
plhs[0] = mxCreateNumericArray(image_ndim, image_dims, mxSINGLE_CLASS, mxREAL);
fimage = (float*)mxGetData(plhs[0]);
ms.GetRawData(fimage);
}
int* labels;
float* modes;
int* count;
int RegionCount = ms.GetRegions(&labels, &modes, &count);
if ( nlhs >= 2 ) {
// second output - labeled image
plhs[1] = mxCreateNumericArray(2, image_dims+1, mxINT32_CLASS, mxREAL);
int* plabels = (int*)mxGetData(plhs[1]);
for (ii=0; ii< w*h; ii++)
plabels[ii] = labels[ii];
}
delete [] labels;
int arr_dims[2];
if ( nlhs >= 3 ) {
// third output - the modes
arr_dims[0] = N;
arr_dims[1] = RegionCount;
plhs[2] = mxCreateNumericArray(2, arr_dims, mxSINGLE_CLASS, mxREAL);
fimage = (float*)mxGetData(plhs[2]);
for (ii=0;ii<N*RegionCount; ii++)
fimage[ii] = modes[ii];
}
delete [] modes;
if ( nlhs >= 4 ) {
// fourth output - region sizes (# of pixels)
arr_dims[0] = 1;
arr_dims[1] = RegionCount;
plhs[3] = mxCreateNumericArray(2, arr_dims, mxINT32_CLASS, mxREAL);
int * pc = (int*)mxGetData(plhs[3]);
for (ii=0;ii<RegionCount; ii++)
pc[ii] = count[ii];
}
delete [] count;
if ( !syn )
return;
if ( nlhs >= 5)
// fifth output - gradient map
plhs[4] = mxgrad;
else
mxDestroyArray(mxgrad);
if ( nlhs >= 6)
plhs[5] = mxconf;
else
mxDestroyArray(mxconf);
}