iCub-main
main.cpp
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2016 iCub Facility - Istituto Italiano di Tecnologia
3 * Author: Marco Randazzo <marco.randazzo@iit.it>
4 * CopyPolicy: Released under the terms of the GPLv2 or later, see GPL.TXT
5 */
6 
7 #include <yarp/sig/Image.h>
8 #include <yarp/os/all.h>
9 
10 #include <iostream>
11 #include <math.h>
12 
13 using namespace yarp::os;
14 using namespace yarp::sig;
15 void printFrame(int h, int w, int c);
16 
17 void merge(const ImageOf<PixelRgb> &imgR, const ImageOf<PixelRgb> &imgL, ImageOf<PixelRgb> &out, size_t start_lx, size_t start_ly, size_t start_rx, size_t start_ry, double alpha1,double alpha2)
18 {
19  size_t max_w = (imgR.width() > imgL.width()) ? imgR.width() : imgL.width();
20  size_t max_h = (imgR.height() > imgL.height()) ? imgR.height() : imgL.height();
21  if (out.width() != max_w || out.height() != max_h) out.resize(max_w, max_h);
22 
23  start_lx = (start_lx < max_w) ? start_lx : max_w;
24  start_ly = (start_ly < max_h) ? start_ly : max_h;
25  start_rx = (start_rx < max_w) ? start_rx : max_w;
26  start_ry = (start_ry < max_h) ? start_ry : max_h;
27  size_t end_lx = (start_lx + imgL.width() < max_w) ? (start_lx + imgL.width()) : max_w;
28  size_t end_ly = (start_ly + imgL.height() < max_h) ? (start_ly + imgL.height()) : max_h;
29  size_t end_rx = (start_rx + imgR.width() < max_w) ? (start_rx + imgR.width()) : max_w;
30  size_t end_ry = (start_ry + imgR.height() < max_h) ? (start_ry + imgR.height()) : max_h;
31 
32  //canvas
33  for (size_t r_dst = 0; r_dst < max_h; r_dst++)
34  {
35  unsigned char *tmp_dst = out.getRow(r_dst);
36 
37  for (size_t c_dst = 0; c_dst < max_w; c_dst++)
38  {
39  tmp_dst[0 + c_dst * 3] = 0;
40  tmp_dst[1 + c_dst * 3] = 0;
41  tmp_dst[2 + c_dst * 3] = 0;
42  }
43  }
44 
45  //left image
46  for (size_t r_dst = start_ly, r_src = 0; r_dst < end_ly; r_dst++, r_src++)
47  {
48  unsigned char *tmp_dst = out.getRow(r_dst);
49  const unsigned char *tmp_src = imgL.getRow(r_src);
50 
51  for (size_t c_dst = start_lx, c_src = 0; c_dst < end_lx; c_dst++, c_src++)
52  {
53  tmp_dst[0 + c_dst * 3] = tmp_dst[0 + c_dst * 3] + (unsigned char)(double(tmp_src[0 + c_src * 3]) * alpha1);
54  tmp_dst[1 + c_dst * 3] = tmp_dst[1 + c_dst * 3] + (unsigned char)(double(tmp_src[1 + c_src * 3]) * alpha1);
55  tmp_dst[2 + c_dst * 3] = tmp_dst[2 + c_dst * 3] + (unsigned char)(double(tmp_src[2 + c_src * 3]) * alpha1);
56  }
57  }
58 
59  //right image
60  for (size_t r_dst = start_ry, r_src = 0; r_dst < end_ry; r_dst++, r_src++)
61  {
62  unsigned char *tmp_dst = out.getRow(r_dst);
63  const unsigned char *tmp_src = imgR.getRow(r_src);
64 
65  for (size_t c_dst = start_rx, c_src = 0; c_dst < end_rx; c_dst++, c_src++)
66  {
67  tmp_dst[0 + c_dst * 3] = tmp_dst[0 + c_dst * 3] + (unsigned char)(double(tmp_src[0 + c_src * 3]) * alpha2);
68  tmp_dst[1 + c_dst * 3] = tmp_dst[1 + c_dst * 3] + (unsigned char)(double(tmp_src[1 + c_src * 3]) * alpha2);
69  tmp_dst[2 + c_dst * 3] = tmp_dst[2 + c_dst * 3] + (unsigned char)(double(tmp_src[2 + c_src * 3]) * alpha2);
70  }
71  }
72 }
73 
74 int main(int argc, char *argv[])
75 {
76  Network yarp;
77  int c=0;
78  ResourceFinder rf;
79  rf.configure(argc, argv);
80 
81  BufferedPort<ImageOf<PixelRgb> > right;
82  BufferedPort<ImageOf<PixelRgb> > left;
83  BufferedPort<ImageOf<PixelRgb> > out;
84 
85  right.open("/imageBlending/right");
86  left.open("/imageBlending/left");
87 
88  out.open("/imageBlending/out");
89 
90  size_t start_rx = 0;
91  size_t start_ry = 0;
92  size_t start_lx = 0;
93  size_t start_ly = 0;
94  double alpha1 = 0.5;
95  double alpha2 = 0.5;
96  if (rf.check("rx")) start_rx = (size_t) rf.find("rx").asFloat64();
97  if (rf.check("ry")) start_ry = (size_t) rf.find("ry").asFloat64();
98  if (rf.check("lx")) start_lx = (size_t) rf.find("lx").asFloat64();
99  if (rf.check("ly")) start_ly = (size_t) rf.find("ly").asFloat64();
100  if (rf.check("alpha1")) alpha1 = rf.find("alpha1").asFloat64();
101  if (rf.check("alpha2")) alpha2 = rf.find("alpha2").asFloat64();
102  yDebug("left offset:%lu,%lu right offset:%lu,%lu, alpha1:%f, alpha2:%f", start_lx, start_ly, start_rx, start_ry, alpha1, alpha2);
103  if (rf.check("help"))
104  {
105  yDebug() << "Available options:";
106  yDebug() << "rx";
107  yDebug() << "ry";
108  yDebug() << "lx";
109  yDebug() << "ly";
110  yDebug() << "alpha1";
111  yDebug() << "alpha2";
112  return 0;
113  }
114 
115  while(true)
116  {
117  ImageOf< PixelRgb> *imgR=right.read(true);
118  ImageOf< PixelRgb> *imgL=left.read(true);
119 
120  ImageOf< PixelRgb> &outImg=out.prepare();
121 
122  if (imgR!=0 && imgL!=0)
123  {
124  merge(*imgR, *imgL, outImg, start_lx, start_ly, start_rx, start_ry,alpha1,alpha2);
125 
126  out.write();
127  c++;
128  }
129  printFrame(outImg.height(), outImg.width(), c);
130  }
131  return 0;
132 }
133 
134 void printFrame(int h, int w, int c)
135 {
136  if (c%500 == 0)
137  printf("Frame #%d %dx%d \n", c, h, w);
138 }
int main(int argc, char *argv[])
Definition: main.cpp:31
void merge(const ImageOf< PixelRgb > &imgR, const ImageOf< PixelRgb > &imgL, ImageOf< PixelRgb > &out, size_t start_lx, size_t start_ly, size_t start_rx, size_t start_ry, double alpha1, double alpha2)
Definition: main.cpp:17
void printFrame(int h, int w, int c)
Definition: main.cpp:134
Copyright (C) 2008 RobotCub Consortium.
out
Definition: sine.m:8