void onRead(T &obj) override
{
if (++cnt==dwnsample)
{
if (firstIncomingData)
{
yInfo() << "Incoming data detected";
firstIncomingData=false;
}
DumpItem item;
Stamp info;
BufferedPort<T>::getEnvelope(info);
item.seqNumber=info.getCount();
if (txTime || (info.isValid() && !rxTime))
item.timeStamp.setTxStamp(info.getTime());
if (rxTime || !info.isValid())
item.timeStamp.setRxStamp(Time::now());
item.obj=factory(obj);
buf.lock();
buf.push_back(item);
buf.unlock();
cnt=0;
}
}
void ThreeDModule::getEncoderPositions(double *headjnt_pos, double *torsojnt_pos, Stamp stamp) {
mutex.wait();
if(stamp.getTime() == 0) {
std::cout << "WARNING: No timestamp found in this bottle!! check your implementation!! " << std::endl;
stamp.update();
}
// store the current (start) indices in the circular buffer
int tIdx = torsoIdx;
int hIdx = headIdx;
// helpers
int idxH = -1, idxT = -1;
int hH, hT;
double smallestDiffH = 10000;
double smallestDiffT = 10000;
double tDiff;
// the bottles to be copied out of the circ buffer
Bottle head, torso;
// going through the circular buffer
idxH = 1;
idxT = 1;
// for(int k = 0; k < LIST_LENGTH; k++) {
// hT = (LIST_LENGTH + tIdx - k) % LIST_LENGTH;
// hH = (LIST_LENGTH + hIdx - k) % LIST_LENGTH;
//
// tDiff = fabs(stamp.getTime() - torsoStamp[hT].getTime());
// std::cout << stamp.getTime() << " idx: " << hT << " getTime: " << torsoStamp[hT].getTime() << ", diffT: "<< tDiff << std::endl;
// if( tDiff < smallestDiffT ) {
// idxT = hT;
// smallestDiffT = tDiff;
// }
//
// tDiff = fabs(stamp.getTime() - torsoStamp[hH].getTime());
// if( tDiff < smallestDiffH ) {
// idxH = hH;
// smallestDiffH = tDiff;
// }
// }
if( idxH != -1 ) head = headState[idxH];
if( idxT != -1 ) torso = torsoState[idxT];
std::cout << "idxT: " << idxT << "/" << idxT-tIdx << " val: " << torso.toString() << std::endl;
std::cout << "idxH: " << idxH << "/" << idxH-hIdx << " val: " << head.toString() << std::endl;
for(int i = 0; i < 6; i++) {
headjnt_pos[i] = head.get(i).asDouble();
if(i < 3) torsojnt_pos[i] = torso.get(i).asDouble();
}
mutex.post();
}
void Rangefinder2DInputPortProcessor::onRead(yarp::os::Bottle &b)
{
now=SystemClock::nowSystem();
mutex.wait();
if (count>0)
{
double tmpDT=now-prev;
deltaT+=tmpDT;
if (tmpDT>deltaTMax)
deltaTMax=tmpDT;
if (tmpDT<deltaTMin)
deltaTMin=tmpDT;
//compare network time
if (tmpDT*1000<LASER_TIMEOUT)
{
state = b.get(1).asInt();
}
else
{
state = IRangefinder2D::DEVICE_TIMEOUT;
}
}
prev=now;
count++;
lastBottle=b;
Stamp newStamp;
getEnvelope(newStamp);
//initialialization (first received data)
if (lastStamp.isValid()==false)
{
lastStamp = newStamp;
}
//now compare timestamps
if ((1000*(newStamp.getTime()-lastStamp.getTime()))<LASER_TIMEOUT)
{
state = b.get(1).asInt();
}
else
{
state = IRangefinder2D::DEVICE_TIMEOUT;
}
lastStamp = newStamp;
mutex.post();
}
void InputPortProcessor::onRead(yarp::sig::Vector &v)
{
now=Time::now();
mutex.wait();
if (count>0)
{
double tmpDT=now-prev;
deltaT+=tmpDT;
if (tmpDT>deltaTMax)
deltaTMax=tmpDT;
if (tmpDT<deltaTMin)
deltaTMin=tmpDT;
//compare network time
if (tmpDT*1000<ANALOG_TIMEOUT)
{
state=IAnalogSensor::AS_OK;
}
else
{
state=IAnalogSensor::AS_TIMEOUT;
}
}
prev=now;
count++;
lastVector=v;
Stamp newStamp;
getEnvelope(newStamp);
//initialialization (first received data)
if (lastStamp.isValid()==false)
{
lastStamp = newStamp;
}
//now compare timestamps
if ((1000*(newStamp.getTime()-lastStamp.getTime()))<ANALOG_TIMEOUT)
{
state=IAnalogSensor::AS_OK;
}
else
{
state=IAnalogSensor::AS_TIMEOUT;
}
lastStamp = newStamp;
mutex.post();
}
bool updateModule()
{
Vector *imuData=iPort.read();
if (imuData==NULL)
return false;
Stamp stamp;
iPort.getEnvelope(stamp);
double t0=Time::now();
Vector gyro=imuData->subVector(6,8);
Vector gyro_filt=gyroFilt.filt(gyro);
gyro-=gyroBias;
gyro_filt-=gyroBias;
Vector mag_filt=magFilt.filt(imuData->subVector(9,11));
double magVel=norm(velEst.estimate(AWPolyElement(mag_filt,stamp.getTime())));
adaptGyroBias=adaptGyroBias?(magVel<mag_vel_thres_up):(magVel<mag_vel_thres_down);
gyroBias=biasInt.integrate(adaptGyroBias?gyro_filt:Vector(3,0.0));
double dt=Time::now()-t0;
if (oPort.getOutputCount()>0)
{
Vector &outData=oPort.prepare();
outData=*imuData;
outData.setSubvector(6,gyro);
oPort.setEnvelope(stamp);
oPort.write();
}
if (bPort.getOutputCount()>0)
{
bPort.prepare()=gyroBias;
bPort.setEnvelope(stamp);
bPort.write();
}
if (verbose)
{
yInfo("magVel = %g => [%s]",magVel,adaptGyroBias?"adapt-gyroBias":"no-adaption");
yInfo("gyro = %s",gyro.toString(3,3).c_str());
yInfo("gyroBias = %s",gyroBias.toString(3,3).c_str());
yInfo("dt = %.0f [us]",dt*1e6);
yInfo("\n");
}
return true;
}
void stereoCalibThread::stereoCalibRun()
{
imageL=new ImageOf<PixelRgb>;
imageR=new ImageOf<PixelRgb>;
Stamp TSLeft;
Stamp TSRight;
bool initL=false;
bool initR=false;
int count=1;
Size boardSize, imageSize;
boardSize.width=this->boardWidth;
boardSize.height=this->boardHeight;
while (!isStopping()) {
ImageOf<PixelRgb> *tmpL = imagePortInLeft.read(false);
ImageOf<PixelRgb> *tmpR = imagePortInRight.read(false);
if(tmpL!=NULL)
{
*imageL=*tmpL;
imagePortInLeft.getEnvelope(TSLeft);
initL=true;
}
if(tmpR!=NULL)
{
*imageR=*tmpR;
imagePortInRight.getEnvelope(TSRight);
initR=true;
}
if(initL && initR && checkTS(TSLeft.getTime(),TSRight.getTime())){
bool foundL=false;
bool foundR=false;
mutex->wait();
if(startCalibration>0) {
string pathImg=imageDir;
preparePath(pathImg.c_str(), pathL,pathR,count);
string iml(pathL);
string imr(pathR);
imgL= (IplImage*) imageL->getIplImage();
imgR= (IplImage*) imageR->getIplImage();
Mat Left(imgL);
Mat Right(imgR);
std::vector<Point2f> pointbufL;
std::vector<Point2f> pointbufR;
if(boardType == "CIRCLES_GRID") {
foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
foundR = findCirclesGridDefault(Right, boardSize, pointbufR, CALIB_CB_SYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else if(boardType == "ASYMMETRIC_CIRCLES_GRID") {
foundL = findCirclesGridDefault(Left, boardSize, pointbufL, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
foundR = findCirclesGridDefault(Right, boardSize, pointbufR, CALIB_CB_ASYMMETRIC_GRID | CALIB_CB_CLUSTERING);
} else {
foundL = findChessboardCorners( Left, boardSize, pointbufL, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
foundR = findChessboardCorners( Right, boardSize, pointbufR, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
}
if(foundL && foundR) {
cvCvtColor(imgL,imgL,CV_RGB2BGR);
cvCvtColor(imgR,imgR, CV_RGB2BGR);
saveStereoImage(pathImg.c_str(),imgL,imgR,count);
imageListR.push_back(imr);
imageListL.push_back(iml);
imageListLR.push_back(iml);
imageListLR.push_back(imr);
Mat cL(pointbufL);
Mat cR(pointbufR);
drawChessboardCorners(Left, boardSize, cL, foundL);
drawChessboardCorners(Right, boardSize, cR, foundR);
count++;
}
if(count>numOfPairs) {
fprintf(stdout," Running Left Camera Calibration... \n");
monoCalibration(imageListL,this->boardWidth,this->boardHeight,this->Kleft,this->DistL);
fprintf(stdout," Running Right Camera Calibration... \n");
monoCalibration(imageListR,this->boardWidth,this->boardHeight,this->Kright,this->DistR);
stereoCalibration(imageListLR, this->boardWidth,this->boardHeight,this->squareSize);
fprintf(stdout," Saving Calibration Results... \n");
updateIntrinsics(imgL->width,imgL->height,Kright.at<double>(0,0),Kright.at<double>(1,1),Kright.at<double>(0,2),Kright.at<double>(1,2),DistR.at<double>(0,0),DistR.at<double>(0,1),DistR.at<double>(0,2),DistR.at<double>(0,3),"CAMERA_CALIBRATION_RIGHT");
updateIntrinsics(imgL->width,imgL->height,Kleft.at<double>(0,0),Kleft.at<double>(1,1),Kleft.at<double>(0,2),Kleft.at<double>(1,2),DistL.at<double>(0,0),DistL.at<double>(0,1),DistL.at<double>(0,2),DistL.at<double>(0,3),"CAMERA_CALIBRATION_LEFT");
Mat Rot=Mat::eye(3,3,CV_64FC1);
Mat Tr=Mat::zeros(3,1,CV_64FC1);
updateExtrinsics(this->R,this->T,"STEREO_DISPARITY");
fprintf(stdout, "Calibration Results Saved in %s \n", camCalibFile.c_str());
startCalibration=0;
count=1;
imageListR.clear();
imageListL.clear();
imageListLR.clear();
}
}
mutex->post();
ImageOf<PixelRgb>& outimL=outPortLeft.prepare();
outimL=*imageL;
outPortLeft.write();
ImageOf<PixelRgb>& outimR=outPortRight.prepare();
outimR=*imageR;
outPortRight.write();
if(foundL && foundR && startCalibration==1)
Time::delay(2.0);
initL=initR=false;
cout.flush();
}
}
delete imageL;
delete imageR;
}
int main(int argc, char *argv[]) {
//Declair yarp
Network yarp;
if (!yarp.checkNetwork())
{
printf("YARP server not available!\n");
return -1;
}
//Creating a Resiving yarp port
BufferedPort<Sound> bufferPort;
bufferPort.open("/receiver");
if (!Network::exists("/receiver")) {
printf("receiver not exists \n");
return -1;
}
//Connecting the resiving audio port with the sender on the pc104
Network::connect("/sender", "/receiver");
if (!Network::isConnected("/sender", "/receiver")) {
printf("no connection \n");
return -1;
}
double empty[samplingBuffer *4] = {0}; //plus 2 for time and sample stamps
//Setup for memory mapping
FILE *fid;
fid = fopen("/tmp/AudioMemMap.tmp", "w");
fwrite(empty, sizeof(double), samplingBuffer * 4, fid);
fclose(fid);
int mappedFileID;
mappedFileID = open("/tmp/AudioMemMap.tmp", O_RDWR);
double *mappedAudioData;
mappedAudioData = (double *)mmap(0, memoryMapSize_bytes, PROT_WRITE, MAP_SHARED , mappedFileID, 0);
//Main loop that maps the sound to the memory mapped region delaired above
Sound* s;
Stamp ts;
while (true) {
//This is a blocking read
s = bufferPort.read(true);
bufferPort.getEnvelope(ts);
printf("count:%d time:%f \n", ts.getCount(), ts.getTime());
int e0 = ts.getCount();
double e1 = ts.getTime();
int row = 0;
for (int col = 0 ; col < samplingBuffer; col++) {
NetInt16 temp_c = (NetInt16) s->get(col, 0);
NetInt16 temp_d = (NetInt16) s->get(col, 1);
mappedAudioData[row] = (double) temp_c / normDivid ;
mappedAudioData[row + 1] = (double) temp_d / normDivid;
mappedAudioData[row + 2] = (double) (e0 * samplingBuffer) + col;
mappedAudioData[row + 3] = (double) (e1 + col * samplePeriod);
row += 4;
}
}
return 0;
}
bool GBSegmModule::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();
cout << "converting image of size " << yrpImgIn->width() << yrpImgIn->height() <<" to size" << input->width() << input->height() << endl;
YarpImageToRGBImage(input, yrpImgIn);
cout << "converted" << endl;
segMutex.wait();
if(seg)
delete seg;
seg=segment_image(input, sigma, k, min_size, &num_components);
segMutex.post();
cout << "processed" << endl;
//prepare timestamps
if(use_private_stamp)
{
_stamp.update();
_viewPort.setEnvelope(_stamp);
}
else
{
_viewPort.setEnvelope(s);
}
ImageOf<PixelRgb> &yrpImgView = _viewPort.prepare();
//Rescale image if required
yrpImgView.resize(seg->width(), seg->height());
RGBImageToYarpImage(seg, &yrpImgView);
_viewPort.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;
}
// Generate new values
void DataPlot::timerEvent(QTimerEvent *)
{
for (int k = 0; k < numberOfInputPorts; k++)
{
int timeout = 0;
int maxTimeout = 2;
Bottle *b = NULL;
while(b==NULL && timeout < maxTimeout)
{
b = inputPorts[k].read(false);
Time::delay(0.005);
timeout++;
}
if (timeout==maxTimeout)
{
if(VERBOSE) fprintf(stderr, "MESSAGE: Couldn't receive data. Going to put a zero! \n");
for(int i = 0; i < numberOfPlots[k]; i++)
{
d_y[k][i][PLOT_SIZE - 1] = 0;
for ( int j = 0; j < PLOT_SIZE - 1; j++ )
{
d_y[k][i][j] = d_y[k][i][j+1];
}
}
}
else
{
for(int i = 0; i < numberOfPlots[k]; i++)
{
Stamp stmp;
inputPorts[k].getEnvelope(stmp);
#ifdef ENABLE_REALTIME
if (stmp.isValid())
d_x_real_time[PLOT_SIZE - 1] = (stmp.getTime() - initialTime);
for ( int j = 0; j < PLOT_SIZE - 1; j++ )
{
if(realTime)
d_x_real_time[j] = d_x_real_time[j+1];
}
#endif
if (b==NULL)
d_y[k][i][PLOT_SIZE - 1] = 0;
else
if (b->size()-1 >= index[k][i])
{
d_y[k][i][PLOT_SIZE - 1] = b->get(index[k][i]).asDouble();
//if(VERBOSE) fprintf(stderr, "MESSAGE: Getting from port %d the index %d\n", k, index[k][i]);
}
// y moves from left to right:
// Shift y array right and assign new value to y[0].
for ( int j = 0; j < PLOT_SIZE - 1; j++ )
{
d_y[k][i][j] = d_y[k][i][j+1];
}
// update the display
//setAxisScale(QwtPlot::yLeft, min, max);
#ifdef ENABLE_REALTIME
if(numberAcquiredData==PLOT_SIZE && realTime)
{
if(VERBOSE) fprintf(stderr, "MESSAGE: switching to real time\n");
QwtPlotCurve *timeBasedCurve = new QwtPlotCurve("Data");
timeBasedCurve->attach(this);
timeBasedCurve->setRawData(d_x_real_time, d_y[k][i], PLOT_SIZE);
timeBasedCurve->setPen(coloredPens[k%NUMBER_OF_LIN][i%NUMBER_OF_COL]);
nonTimeBasedCurve[k][i].attach(NULL);
//Set title
char cTitle[256];
sprintf(cTitle, "Data(%d)", index[k][i]);
QwtText curveTitle(cTitle, QwtText::PlainText);
curveTitle.setFont(plotFont);
timeBasedCurve->setTitle(curveTitle);
}
#endif
}
}
}
if (acquire)
replot();
numberAcquiredData++;
//if(VERBOSE) fprintf(stderr, "Number of acquired data is %d\n", numberAcquiredData);
//QSize plotSize= this->sizeHint();
//if(VERBOSE) fprintf(stderr, "Hint is: hInt=%d, vInt=%d\n", plotSize.height(), plotSize.width());
static double before = Time::now();
double now = Time::now();
static double meanEffectiveTime = 0;
double currentEffectiveTime = (now - before)*1000.0;
if (numberAcquiredData >= 2)
meanEffectiveTime = (meanEffectiveTime*(numberAcquiredData-2) + currentEffectiveTime)/(numberAcquiredData-1);
//if(VERBOSE) fprintf(stderr, "Iteration %d: Current time is %f and mean is %f\n", numberAcquiredData, currentEffectiveTime, meanEffectiveTime);
if (currentEffectiveTime*0.5 > d_interval)
{
if(VERBOSE) fprintf(stderr, "Real Timer is %f but I was supposed to run at %d ms. Mean is: %f \n", currentEffectiveTime, d_interval, meanEffectiveTime);
if(VERBOSE) fprintf(stderr, "You should slow down to %d ms \n", (int) (meanEffectiveTime * 2));
//setTimerInterval((int) (meanEffectiveTime * 2));
}
before = now;
}
void DataPlot::initSignalDimensions()
{
if(VERBOSE) fprintf(stderr, "MESSAGE: Will now initialize the signal dimensions\n");
//getting info on the connected port
nonTimeBasedCurve = new QwtPlotCurve*[numberOfInputPorts];
for (int k = 0; k < numberOfInputPorts; k++)
{
Bottle *b = NULL;
int timeout = 0;
const int maxTimeout = 1000;
while(b==NULL && timeout < maxTimeout)
{
b = inputPorts[k].read(false);
Time::delay(0.005);
timeout++;
}
if (timeout==maxTimeout)
{
if(VERBOSE) fprintf(stderr, "MESSAGE: Couldn't receive data. Going to put a zero! \n");
realTime = false;
// Initialize data
for (int j = 0; j< numberOfPlots[k]; j++)
{
for (int i = 0; i < PLOT_SIZE; i++)
{
d_x[i] = i; // time axis
//if(VERBOSE) fprintf(stderr, "MESSAGE: (%d, %d)\n", j, i);
d_y[k][j][i] = 0;
}
}
}
else
{
if(VERBOSE) fprintf(stderr, "MESSAGE: Will now try real time!\n");
inputVectorSize = b->size();
Stamp stmp;
inputPorts[k].getEnvelope(stmp);
if (stmp.isValid())
{
if(VERBOSE) fprintf(stderr, "MESSAGE: will use real time!\n");
realTime = true;
initialTime = stmp.getTime();
}
// Initialize data
for (int j = 0; j< numberOfPlots[k]; j++)
{
if (b->size()-1 < index[k][j])
if(VERBOSE) fprintf(stderr, "WARNING: will plot some zeros since the accessed index exceed the input vector dimensions!\n");
for (int i = 0; i < PLOT_SIZE; i++)
{
d_x[i] = i; // time axis
#ifdef ENABLE_REALTIME
if (realTime)
d_x_real_time[i] = i;
#endif
//if(VERBOSE) fprintf(stderr, "MESSAGE: (%d, %d)\n", j, i);
d_y[k][j][i] = 0;
}
}
}
//if(VERBOSE) fprintf(stderr, "MESSAGE: initializing plot datas!\n");
// Assign a title
insertLegend(new QwtLegend(), QwtPlot::BottomLegend);
nonTimeBasedCurve[k] = new QwtPlotCurve[numberOfPlots[k]];
for(int i=0; i < numberOfPlots[k]; i++)
{
//Set title
char cTitle[256];
sprintf(cTitle, "Data(%d)", index[k][i]);
QwtText curveTitle(cTitle, QwtText::PlainText);
curveTitle.setFont(plotFont);
nonTimeBasedCurve[k][i].setTitle(curveTitle);
//if(VERBOSE) fprintf(stderr, "MESSAGE: Will now initialize the plot %d\n", index[i]);
// Insert new curves
nonTimeBasedCurve[k][i].attach(this);
// Set curve styles
nonTimeBasedCurve[k][i].setPen(coloredPens[k%NUMBER_OF_LIN][i%NUMBER_OF_COL]);
// Attach (don't copy) data. Both curves use the same x array.
nonTimeBasedCurve[k][i].setRawData(d_x, d_y[k][i], PLOT_SIZE);
}
// Axis
QwtText axisTitle("Time/seconds");
axisTitle.setFont(plotFont);
setAxisTitle(QwtPlot::xBottom, axisTitle);
setAxisScale(QwtPlot::xBottom, 0, 100);
setAxisFont(QwtPlot::xBottom, plotFont);
setAxisTitle(QwtPlot::yLeft, "Values");
//setAxisScale(QwtPlot::yLeft, -1.5, 1.5);
setAxisAutoScale(QwtPlot::yLeft);
setAxisAutoScale(QwtPlot::xBottom);
setAxisFont(QwtPlot::yLeft, plotFont);
setTimerInterval(50.0);
}
//if(VERBOSE) fprintf(stderr, "MESSAGE: plot intialized!\n");
}
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;
}
