virtual bool threadInit() override
{
p.open("/readWriteTest_writer");
image.resize(100, 100);
image.zero();
return true;
}
void QGLVideo::paintImage(yarp::sig::ImageOf<yarp::sig::PixelRgb> &img){
int wWidth = this->width();
int wHeight = this->height();
int width = img.width();
int height = img.height();
if(width == 0 || height == 0){
return;
}
if (!(wWidth == _yrpImgCache.width() || wHeight == _yrpImgCache.height())) {
double ratioWindow = (double)wWidth/(double)wHeight;
double ratioImage = (double)width/(double)height;
_yrpImgCache.setQuantum(1);
_yrpImgCache.setTopIsLowIndex(false);
if(ratioWindow > ratioImage){
// need to stretch height
_yrpImgCache.resize((int)(((double)width)*((double)wHeight/(double)height)), wHeight);
}
else{
// need to stretch width
_yrpImgCache.resize(wWidth, (int)(((double)height)*((double)wWidth/(double)width)));
}
_yrpImgCache.zero();
}
_yrpImgCache.copy(img, _yrpImgCache.width(), _yrpImgCache.height()); // scale input image (if required)
_pixData = (unsigned char*)_yrpImgCache.getRawImage();
this->updateGL();
}
bool USBCameraDriverRgb::getImage(yarp::sig::ImageOf<yarp::sig::PixelRgb>& image)
{
if( (image.width() != _width) || (image.height() != _height) )
image.resize(_width, _height);
deviceRgb->getRgbBuffer(image.getRawImage());
return true;
}
bool MapGrid2D::setOccupancyGrid(yarp::sig::ImageOf<yarp::sig::PixelMono>& image)
{
if (image.width() != m_width ||
image.height() != m_height)
{
yError() << "The size of given occupancy grid does not correspond to the current map. Use method setSize() first.";
return false;
}
m_map_occupancy = image;
return true;
}
void copyImage(yarp::sig::ImageOf<yarp::sig::PixelRgb>& src,
Magick::Image& dest) {
int h = src.height();
int w = src.width();
dest.size(Magick::Geometry(w,h));
dest.depth(8);
for (int i=0; i<h; i++) {
// must transfer row by row, since YARP may use padding in representation
Magick::PixelPacket *packet = dest.setPixels(0,i,w,1);
dest.readPixels(Magick::RGBQuantum,(unsigned char *)(&src.pixel(0,i)));
}
dest.syncPixels();
}
bool MapGrid2D::getMapImage(yarp::sig::ImageOf<PixelRgb>& image) const
{
image.setQuantum(1);
image.resize(m_width, m_height);
image.zero();
for (size_t y = 0; y < m_height; y++)
{
for (size_t x = 0; x < m_width; x++)
{
image.safePixel(x, y) = CellDataToPixel(m_map_flags.safePixel(x, y));
}
}
return true;
}
void copyImage(Magick::Image& src,
yarp::sig::ImageOf<yarp::sig::PixelRgb>& dest) {
Magick::Geometry g = src.size();
int h = g.height();
int w = g.width();
src.depth(8);
dest.resize(w,h);
for (int i=0; i<h; i++) {
// must transfer row by row, since YARP may use padding in representation
Magick::PixelPacket *packet = src.getPixels(0,i,w,1);
src.writePixels(Magick::RGBQuantum,(unsigned char *)(&dest.pixel(0,i)));
}
src.syncPixels();
}
void TrackerPool::display(yarp::sig::ImageOf<yarp::sig::PixelRgb> &img){
for(int ii=0; ii<trackers_.size(); ii++){
if(trackers_[ii].is_active()){
//yarp::sig::PixelRgb color = trackers_[ii].is_active()?yarp::sig::PixelRgb(0, 0, 255):yarp::sig::PixelRgb(0, 255, 0);
yarp::sig::PixelRgb color = yarp::sig::PixelRgb(0, 0, 255);
trackers_[ii].display(img, color);
}
}
std::vector<int> to_delete;
for(int ii=0; ii<collisions_disp_.size(); ii++){
for(int jj=-1; jj<1; jj++){
for(int kk=-1; kk<1; kk++){
int x = collisions_disp_[ii].x + jj;
int y = collisions_disp_[ii].y + kk;
if(x>=0 && x<128 && y>=0 && y<128){
img.pixel(y, x) = yarp::sig::PixelRgb(20, 0, 255);
}
}
}
if(++collisions_disp_[ii].count_disp == 100){
to_delete.push_back(ii);
}
}
while(to_delete.size()>0){
int to_del = to_delete.back();
collisions_disp_.erase(collisions_disp_.begin() + to_del);
to_delete.pop_back();
}
}
bool MapGrid2D::setMapImage(yarp::sig::ImageOf<PixelRgb>& image)
{
if (image.width() != (int)(m_width) ||
image.height() != (int)(m_height))
{
yError() << "The size of given iamge does not correspond to the current map. Use method setSize() first.";
return false;
}
for (size_t y = 0; y < m_height; y++)
{
for (size_t x = 0; x < m_width; x++)
{
m_map_flags.safePixel(x, y) = PixelToCellData(image.safePixel(x, y));
}
}
return true;
}
bool VfwGrabber::getImage(yarp::sig::ImageOf<yarp::sig::PixelRgb>& image) {
icvGrabFrameCAM_VFW(&HELPER(system_resource));
Image *img = icvRetrieveFrameCAM_VFW(&HELPER(system_resource));
//printf("image size %d %d\n", img->width(), img->height());
image.copy(*img);
_width = img->width();
_height = img->height();
return img->width()>0;
}
bool realsense2Driver::getImage(yarp::sig::ImageOf<yarp::sig::PixelMono>& image)
{
if (!m_stereoMode)
{
yError()<<"realsense2Driver: infrared stereo stream not enabled";
return false;
}
image.resize(width(), height());
std::lock_guard<std::mutex> guard(m_mutex);
rs2::frameset data = m_pipeline.wait_for_frames();
rs2::video_frame frm1 = data.get_infrared_frame(1);
rs2::video_frame frm2 = data.get_infrared_frame(2);
int pixCode = pixFormatToCode(frm1.get_profile().format());
if (pixCode != VOCAB_PIXEL_MONO && pixCode != VOCAB_PIXEL_MONO16)
{
yError() << "realsense2Driver: expecting Pixel Format MONO or MONO16";
return false;
}
size_t singleImage_rowSizeByte = image.getRowSize()/2;
unsigned char * pixelLeft = (unsigned char*) (frm1.get_data());
unsigned char * pixelRight = (unsigned char*) (frm2.get_data());
unsigned char * pixelOutLeft = image.getRawImage();
unsigned char * pixelOutRight = image.getRawImage() + singleImage_rowSizeByte;
for (size_t h=0; h< image.height(); h++)
{
memcpy(pixelOutLeft, pixelLeft, singleImage_rowSizeByte);
memcpy(pixelOutRight, pixelRight, singleImage_rowSizeByte);
pixelOutLeft += 2*singleImage_rowSizeByte;
pixelOutRight += 2*singleImage_rowSizeByte;
pixelLeft += singleImage_rowSizeByte;
pixelRight += singleImage_rowSizeByte;
}
return true;
}
bool YarpCam::getImage ( yarp::sig::ImageOf< yarp::sig::PixelRgb >& image ) {
char* buffer;
unsigned size;
if ( !_camera->GetImage ( &buffer, size ) )
return false;
image.setExternal(buffer,width(),height());
return true;
}
bool yarp::dev::GazeboYarpMultiCameraDriver::getImage(yarp::sig::ImageOf<yarp::sig::PixelRgb>& _image)
{
for (unsigned int i = 0; i < m_camera_count; ++i) {
m_dataMutex[i]->wait();
}
if (m_vertical) {
_image.resize(m_max_width, m_max_height * m_camera_count);
} else {
_image.resize(m_max_width * m_camera_count, m_max_height);
}
unsigned char *pBuffer = _image.getRawImage();
memset(pBuffer, 0, _image.getRawImageSize());
for (unsigned int i = 0; i < m_camera_count; ++i) {
int rowsize = 3 * m_width[i];
int rowoffset = i * 3 * m_max_width;
if (m_vertical) {
if (m_max_width == m_width[i] && m_max_height == m_height[i]) {
memcpy(pBuffer + (3 * m_max_width * m_max_height * i), m_imageBuffer[i], m_bufferSize[i]);
} else {
for (int r = 0; r < m_height[i]; r++) {
memcpy(pBuffer + (3 * m_max_width) * (m_max_height * i + r), m_imageBuffer[i] + rowsize * r, rowsize);
}
}
} else {
for (int r = 0; r < m_height[i]; r++) {
memcpy(pBuffer + (3 * m_max_width * m_camera_count * r) + rowoffset, m_imageBuffer[i] + rowsize * r, rowsize);
}
}
}
if (m_display_time_box) {
m_counter = (++m_counter % 10);
for (int c = m_counter*30; c < 30 + m_counter*30; c++) {
for (int r=0; r<30; r++) {
for (unsigned int i = 0; i < m_camera_count; ++i) {
unsigned char *pixel;
if (m_vertical) {
pixel = _image.getPixelAddress(m_width[i]-c-1, m_max_height * i + m_height[i]-r-1);
} else {
pixel = _image.getPixelAddress(m_max_width * i + m_width[i]-c-1, m_height[i]-r-1);
}
pixel[0] = (m_counter % 2 == 0) ? 255 : 0;
pixel[1] = (m_counter % 2 == 0) ? 0 : 255;
pixel[2] = 0;
}
}
}
}
for (unsigned int i = 0; i < m_camera_count; ++i) {
m_dataMutex[i]->post();
}
return true;
}
bool BayerCarrier::debayerHalf(yarp::sig::ImageOf<PixelMono>& src,
yarp::sig::ImageOf<PixelRgb>& dest) {
// dc1394 doesn't seem safe for arbitrary data widths
if (src.width()%8==0) {
dc1394video_frame_t dc_src;
dc1394video_frame_t dc_dest;
setDcImage(src,&dc_src,dcformat);
setDcImage(dest,&dc_dest,dcformat);
dc1394_debayer_frames(&dc_src,&dc_dest,DC1394_BAYER_METHOD_DOWNSAMPLE);
return true;
}
if (bayer_method_set && !warned) {
fprintf(stderr, "Not using dc1394 debayer methods (image width not a multiple of 8)\n");
warned = true;
}
// a safer implementation that doesn't use dc1394
int w = src.width();
int h = src.height();
int wo = dest.width();
int ho = dest.height();
int goff1 = 1-goff;
int roffx = roff?goff:goff1;
int boff = 1-roff;
int boffx = boff?goff:goff1;
for (int yo=0; yo<ho; yo++) {
for (int xo=0; xo<wo; xo++) {
PixelRgb& po = dest.pixel(xo,yo);
int x = xo*2;
int y = yo*2;
if (x+1>=w-1 || y+1>=h-1) {
po = PixelRgb(0,0,0);
continue;
}
po.r = src.pixel(x+roffx,y+roff);
po.b = src.pixel(x+boffx,y+boff);
po.g = (PixelMono)(0.5*(src.pixel(x+goff,y)+src.pixel(x+goff1,y+1)));
}
}
return true;
}
// Inactive trackers are displayed in blue, active trackers in red
void BlobTracker::display(yarp::sig::ImageOf<yarp::sig::PixelRgb> &img, yarp::sig::PixelRgb color){
Mat orig = (IplImage *)img.getIplImage();
double a, b, alpha;
this->get_ellipse_parameters(a, b, alpha);
a*=k_;
b*=k_;
alpha = alpha * 180 / M_PI; //convert to degrees for openCV ellipse function
//open CV for drawing ellipse
ellipse(orig, Point(cen_y_,cen_x_), Size(a,b), alpha, 0, 360, Scalar(255,0,0));
//putText(orig, "act", Point(cen_y_,cen_x_), 1, 1, Scalar(255,0,0));
}
void InputCallback::onRead(yarp::sig::ImageOf<yarp::sig::PixelRgba> &img)
#endif
{
uchar *tmpBuf;
QSize s = (QSize(img.width(),img.height()));
#if QT_VERSION >= 0x050302
int imgSize = img.getRawImageSize();
#else
int imgSize = s.width() * s.height() * img.getPixelSize();
#endif
// Allocate a QVideoFrame
QVideoFrame frame(imgSize,
s,
#if QT_VERSION >= 0x050302
img.getRowSize(),
#else
s.width() * img.getPixelSize(),
#endif
QVideoFrame::Format_RGB32);
// Maps the buffer
frame.map(QAbstractVideoBuffer::WriteOnly);
// Takes the ownership of the buffer in write only mode
tmpBuf = frame.bits();
unsigned char *rawImg = img.getRawImage();
//int j = 0;
// Inverts the planes because Qt Wants an image in RGB format instead of BGR
/* for(int i=0; i<imgSize; i++){
tmpBuf[j+2] = rawImg[i];
i++;
tmpBuf[j+1] = rawImg[i];
i++;
tmpBuf[j] = rawImg[i];
tmpBuf[j+3] = 0;
j+=4;
}*/
#if QT_VERSION >= 0x050302
memcpy(tmpBuf,rawImg,imgSize);
#else
for(int x =0; x < s.height(); x++) {
memcpy(tmpBuf + x * (img.width() * img.getPixelSize()),
rawImg + x * (img.getRowSize()),
img.width() * img.getPixelSize());
}
#endif
//unmap the buffer
frame.unmap();
if(sigHandler){
sigHandler->sendVideoFrame(frame);
}
}
void TestFrameGrabber::createTestImage(yarp::sig::ImageOf<yarp::sig::PixelRgb>&
image) {
// to test IPreciselyTimed, make timestamps be mysteriously NNN.NNN42
double t = Time::now();
t -= ((t*1000)-(int)t)/1000;
t+= 0.00042;
stamp.update(t);
if (background.width()>0) {
image.copy(background);
} else {
image.zero();
image.resize(w,h);
}
switch (mode) {
case VOCAB_BALL:
{
addCircle(image,PixelRgb(0,255,0),bx,by,15);
addCircle(image,PixelRgb(0,255,255),bx,by,8);
if (ct%5!=0) {
rnd *= 65537;
rnd += 17;
bx += (rnd%5)-2;
rnd *= 65537;
rnd += 17;
by += (rnd%5)-2;
} else {
int dx = w/2 - bx;
int dy = h/2 - by;
if (dx>0) { bx++; }
if (dx<0) { bx--; }
if (dy>0) { by++; }
if (dy<0) { by--; }
}
}
break;
case VOCAB_GRID:
{
int ww = image.width();
int hh = image.height();
if (ww>1&&hh>1) {
for (int x=0; x<ww; x++) {
for (int y=0; y<hh; y++) {
double xx = ((double)x)/(ww-1);
double yy = ((double)y)/(hh-1);
int r = int(0.5+255*xx);
int g = int(0.5+255*yy);
bool act = (y==ct);
image.pixel(x,y) = PixelRgb(r,g,act*255);
}
}
}
}
break;
case VOCAB_LINE:
default:
{
for (int i=0; i<image.width(); i++) {
image.pixel(i,ct).r = 255;
}
}
break;
case VOCAB_RAND:
{
// from Alessandro Scalzo
static unsigned char r=128,g=128,b=128;
int ww = image.width();
int hh = image.height();
if (ww>1&&hh>1) {
for (int x=0; x<ww; x++) {
for (int y=0; y<hh; y++) {
//r+=(rand()%3)-1;
//g+=(rand()%3)-1;
//b+=(rand()%3)-1;
r += Random::uniform(-1,1);
g += Random::uniform(-1,1);
b += Random::uniform(-1,1);
image.pixel(x,y) = PixelRgb(r,g,b);
}
}
}
}
break;
case VOCAB_NONE:
break;
}
ct++;
if (ct>=image.height()) {
ct = 0;
}
if (by>=image.height()) {
by = image.height()-1;
}
if (bx>=image.width()) {
bx = image.width()-1;
}
if (bx<0) bx = 0;
if (by<0) by = 0;
}
bool BayerCarrier::debayerFull(yarp::sig::ImageOf<PixelMono>& src,
yarp::sig::ImageOf<PixelRgb>& dest) {
// dc1394 doesn't seem safe for arbitrary data widths
if (src.width()%8==0) {
dc1394video_frame_t dc_src;
dc1394video_frame_t dc_dest;
setDcImage(src,&dc_src,dcformat);
setDcImage(dest,&dc_dest,dcformat);
dc1394_debayer_frames(&dc_src,&dc_dest,
(dc1394bayer_method_t)bayer_method);
return true;
}
if (bayer_method_set && !warned) {
fprintf(stderr, "Not using dc1394 debayer methods (image width not a multiple of 8)\n");
warned = true;
}
int w = dest.width();
int h = dest.height();
int goff1 = 1-goff;
int roffx = roff?goff:goff1;
int boff = 1-roff;
int boffx = boff?goff:goff1;
for (int y=0; y<h; y++) {
for (int x=0; x<w; x++) {
PixelRgb& po = dest.pixel(x,y);
// G
if ((x+y)%2==goff) {
po.g = src.pixel(x,y);
} else {
float g = 0;
int ct = 0;
if (x>0) { g += src.pixel(x-1,y); ct++; }
if (x<w-1) { g += src.pixel(x+1,y); ct++; }
if (y>0) { g += src.pixel(x,y-1); ct++; }
if (y<h-1) { g += src.pixel(x,y+1); ct++; }
if (ct>0) g /= ct;
po.g = (int)g;
}
// B
if (y%2==boff && x%2==boffx) {
po.b = src.pixel(x,y);
} else if (y%2==boff) {
float b = 0;
int ct = 0;
if (x>0) { b += src.pixel(x-1,y); ct++; }
if (x<w-1) { b += src.pixel(x+1,y); ct++; }
if (ct>0) b /= ct;
po.b = (int)b;
} else if (x%2==boffx) {
float b = 0;
int ct = 0;
if (y>0) { b += src.pixel(x,y-1); ct++; }
if (y<h-1) { b += src.pixel(x,y+1); ct++; }
if (ct>0) b /= ct;
po.b = (int)b;
} else {
float b = 0;
int ct = 0;
if (x>0&&y>0) { b += src.pixel(x-1,y-1); ct++; }
if (x>0&&y<h-1) { b += src.pixel(x-1,y+1); ct++; }
if (x<w-1&&y>0) { b += src.pixel(x+1,y-1); ct++; }
if (x<w-1&&y<h-1) { b += src.pixel(x+1,y+1); ct++; }
if (ct>0) b /= ct;
po.b = (int)b;
}
// R
if (y%2==roff && x%2==roffx) {
po.r = src.pixel(x,y);
} else if (y%2==roff) {
float r = 0;
int ct = 0;
if (x>0) { r += src.pixel(x-1,y); ct++; }
if (x<w-1) { r += src.pixel(x+1,y); ct++; }
if (ct>0) r /= ct;
po.r = (int)r;
} else if (x%2==roffx) {
float r = 0;
int ct = 0;
if (y>0) { r += src.pixel(x,y-1); ct++; }
if (y<h-1) { r += src.pixel(x,y+1); ct++; }
if (ct>0) r /= ct;
po.r = (int)r;
} else {
float r = 0;
int ct = 0;
if (x>0&&y>0) { r += src.pixel(x-1,y-1); ct++; }
if (x>0&&y<h-1) { r += src.pixel(x-1,y+1); ct++; }
if (x<w-1&&y>0) { r += src.pixel(x+1,y-1); ct++; }
if (x<w-1&&y<h-1) { r += src.pixel(x+1,y+1); ct++; }
if (ct>0) r /= ct;
po.r = (int)r;
}
}
}
return true;
}
bool FfmpegGrabber::getAudioVisual(yarp::sig::ImageOf<yarp::sig::PixelRgb>& image,
yarp::sig::Sound& sound) {
FfmpegHelper& helper = HELPER(system_resource);
DecoderState& videoDecoder = helper.videoDecoder;
DecoderState& audioDecoder = helper.audioDecoder;
bool tryAgain = false;
bool triedAgain = false;
do {
bool gotAudio = false;
bool gotVideo = false;
if (startTime<0.5) {
startTime = Time::now();
}
double time_target = 0;
while(av_read_frame(pFormatCtx, &packet)>=0) {
// Is this a packet from the video stream?
DBG printf("frame ");
bool done = false;
if (packet.stream_index==videoDecoder.getIndex()) {
DBG printf("video ");
done = videoDecoder.getVideo(packet);
image.resize(1,1);
if (done) {
//printf("got a video frame\n");
gotVideo = true;
}
} if (packet.stream_index==audioDecoder.getIndex()) {
DBG printf("audio ");
done = audioDecoder.getAudio(packet,sound);
if (done) {
//printf("got an audio frame\n");
gotAudio = true;
}
} else {
DBG printf("other ");
}
AVRational& time_base = pFormatCtx->streams[packet.stream_index]->time_base;
double rbase = av_q2d(time_base);
DBG printf(" time=%g ", packet.pts*rbase);
time_target = packet.pts*rbase;
av_free_packet(&packet);
DBG printf(" %d\n", done);
if (((imageSync?gotVideo:videoDecoder.haveFrame())||!_hasVideo)&&
((imageSync?1:gotAudio)||!_hasAudio)) {
if (_hasVideo) {
videoDecoder.getVideo(image);
} else {
image.resize(0,0);
}
if (needRateControl) {
double now = (Time::now()-startTime)*pace;
double delay = time_target-now;
if (delay>0) {
DBG printf("DELAY %g ", delay);
Time::delay(delay);
} else {
DBG printf("NODELAY %g ", delay);
}
}
DBG printf("IMAGE size %dx%d ", image.width(), image.height());
DBG printf("SOUND size %d\n", sound.getSamples());
if (!_hasAudio) {
sound.resize(0,0);
}
return true;
}
}
tryAgain = !triedAgain;
if (tryAgain) {
if (!shouldLoop) {
return false;
}
av_seek_frame(pFormatCtx,-1,0,AVSEEK_FLAG_BACKWARD);
startTime = Time::now();
}
} while (tryAgain);
return false;
}
