void SetJoints(JointPos& j)
{
mutex.Wait();
state.joints = j;
last_neck_time = YARPTime::GetTimeAsSeconds();
mutex.Post();
}
void SetGyro(double n_yaw, double n_pitch, double n_roll)
{
mutex.Wait();
state.yaw = n_yaw; state.pitch = n_pitch; state.roll = n_roll;
last_gyro_time = YARPTime::GetTimeAsSeconds();
mutex.Post();
}
void MarkTarget(int id)
{
printf("Marking current region as target %d\n", id);
state_mutex.Wait();
target_manager.Set(id,global_theta,global_phi);
state_mutex.Post();
}
int YARPSocketNameService::RegisterName(const char *name)
{
int name_status = -1;
int result = -1;
//static YARPInputSocket sock;
//sock.Register(name);
//test_global = &sock;
DBG(5) printf("^^^^^^^^ checking for previous definitions\n");
if (GetThreadSocket()==NULL)
{
DBG(5) printf("^^^^^^^^ checks out okay\n");
int pid = my_getpid();
mutex.Wait();
DBG(5) printf("^^^^^^^^ creating\n");
YARPInputSocket *is = new YARPInputSocket;
is_map[pid] = is;
DBG(5) printf("^^^^^^^^ preparing to register\n");
is->Register(name);
result = is->GetAssignedPort();
mutex.Post();
DBG(5) printf("^^^^^^^^ Made an input socket handler\n");
fflush(stdout);
}
return result;
}
virtual void OnRead()
{
assert(Read(0));
state_mutex.Wait();
state_joint = Content();
state_mutex.Post();
}
int Get(HeadState& dest)
{
int result = IsValid();
mutex.Wait();
dest = state;
mutex.Post();
return result;
}
void SetEyes(double n_tilt, double n_left_pan, double n_right_pan)
{
mutex.Wait();
state.tilt = n_tilt; state.left_pan = n_left_pan;
state.right_pan = n_right_pan;
last_eyes_time = YARPTime::GetTimeAsSeconds();
mutex.Post();
}
void ResetControl()
{
mutex.Wait();
HeadMessage& msg = out_cmd.Content();
msg.type = HeadMsgNonValid;
out_cmd.Write();
mutex.Post();
}
int IsValid()
{
double now = YARPTime::GetTimeAsSeconds();
mutex.Wait();
int result = /*(now-last_gyro_time<1)&&*/(now-last_eyes_time<1)&&
(now-last_neck_time<1);
mutex.Post();
return result;
}
// LATER: find out why this is commented out.
static void RemovePort(YARPPort *port)
{
ACE_UNUSED_ARG(port);
port_list_mutex.Wait();
//#ifndef __QNX__
// port_list.erase(port);
//#endif
port_list_mutex.Post();
}
void SetNeck(double n_tilt, double n_roll,
double n_lean, double n_pan)
{
mutex.Wait();
state.neck_tilt = n_tilt;
state.neck_roll = n_roll;
state.neck_lean = n_lean;
state.neck_pan = n_pan;
//last_neck_time = YARPTime::GetTimeAsSeconds();
mutex.Post();
}
YARPInputSocket *GetThreadSocket()
{
int pid = my_getpid();
YARPInputSocket *result = NULL;
mutex.Wait();
if (is_map.find(pid)!=is_map.end())
{
result = is_map[pid];
}
mutex.Post();
return result;
}
void YARPPort::DeactivateAll()
{
port_list_mutex.Wait();
PortList::iterator it(port_list);
it.go_head(); // might not be required.
while (!it.done())
{
(*it)->Deactivate();
it++;
}
port_list_mutex.Post();
}
void RequestOrientation(double theta, double phi)
{
mutex.Wait();
HeadMessage& msg = out_cmd.Content();
msg.type = HeadMsgMoveToOrientation;
msg.j1 = theta;
msg.j2 = phi;
msg.j3 = 0;
msg.j4 = theta;
msg.j5 = phi;
msg.j6 = 0;
msg.j7 = 0;
out_cmd.Write();
mutex.Post();
}
int main()
{
in_gyro.Register("/egomap/i:gyro");
in_eye.Register("/egomap/i:eye");
out_dir.Register("/egomap/o:dir");
while (1)
{
wake_up_call.Wait();
HeadState state;
int ok = head_state_manager.Get(state);
if (ok)
{
CogGaze dir;
dir.Apply(state.joints);
printf("LEFT %+8.2f %+8.2f %+8.2f RIGHT %+8.2f %+8.2f %+8.2f\n",
dir.theta_left*180.0/M_PI,
dir.phi_left*180.0/M_PI,
dir.roll_left*180.0/M_PI,
dir.theta_right*180.0/M_PI,
dir.phi_right*180.0/M_PI,
dir.roll_right*180.0/M_PI);
}
//YARPTime::DelayInSeconds(0.05);
}
return 0;
}
//--------------------------------------------------------------------------------------
// Class: SoundResources
// Method: (public)_uninitialize
// Description:
//--------------------------------------------------------------------------------------
int
SoundResources::_uninitialize (void)
{
_bmutex.Wait ();
m_InRecord = false;
mixerClose(m_MixerHandle); // Close mixer
waveInReset(m_WaveInHandle); // Reset the wave input device
if(_rawBuffer != NULL){
delete[] _rawBuffer; // Delete the shared buffer
_rawBuffer = NULL;
}
_bmutex.Post ();
return YARP_OK;
}
inline int PicoloResources::_init (const PicoloOpenParameters& params)
{
/// copy params.
int ret;
_nRequestedSize = params._size_x;
_nWidth = params._size_x;
_nHeight = params._size_y;
_nImageSize = params._size_x * params._size_y * 3;
init_bttvx(params._video_type,params._unit_number,_nWidth,_nHeight, params._offset_x, params._offset_y);
_bmutex.Wait ();
_rawBuffer = new unsigned char [_nImageSize];
ACE_ASSERT (_rawBuffer != NULL);
_bmutex.Post ();
return ret;
}
inline int PicoloResources::_uninitialize (void)
{
_bmutex.Wait ();
close_bttvx();
if (_rawBuffer != NULL) delete[] _rawBuffer;
_rawBuffer = NULL;
_nRequestedSize = 0;
_nWidth = 0;
_nHeight = 0;
_nImageSize = 0;
_canpost = true;
_bmutex.Post ();
return YARP_OK;
}
void BeginObservations()
{
if (!observing)
{
mutex.Wait();
UpdateActivity();
observing = 1;
}
}
virtual void OnRead()
{
assert(Read(0));
JointPos& datum = Content();
// printf("eye %g %g %g\n", datum(1), datum(2), datum(3));
head_state_manager.SetEyes(datum(1), datum(2), datum(3));
// head_state_manager.SetNeck(datum(6), datum(7), datum(4), datum(5));
head_state_manager.SetNeck(datum(4), datum(5), datum(7), datum(6));
head_state_manager.SetJoints(datum);
wake_up_call.Post();
}
virtual void Body()
{
char buf[128] = "Not set";
char reply[128] = "Not set";
int ct = 0;
YARPTime::DelayInSeconds(0.01);
/// connect to name server and get ip and port.
#ifdef __QNX6__
YARPUniqueNameID* id = YARPNameService::RegisterName(REG_TEST_NAME, NET_NAME, YARP_QNET, YARPNativeEndpointManager::CreateQnetChannel());
#else
YARPUniqueNameID* id = YARPNameService::RegisterName(REG_TEST_NAME, NET_NAME, YARP_UDP, 11);
#endif
if (id->getServiceType() == YARP_NO_SERVICE_AVAILABLE)
{
ACE_DEBUG ((LM_DEBUG, "can't register name, bailing out\n"));
return;
}
/// create the input endpoint.
YARPEndpointManager::CreateInputEndpoint (*id);
while (!IsTerminated())
{
out.Wait();
cout << "Waiting for input" << endl;
cout.flush();
out.Post();
YARPNameID reply_id = YARPSyncComm::BlockingReceive(id->getNameID(), buf, sizeof(buf));
sprintf(reply,"Got %s", buf);
out.Wait();
cout << "Received message: " << buf << endl;
cout.flush();
out.Post();
YARPSyncComm::Reply(reply_id, reply, sizeof(reply));
ct++;
}
}
void RevertTarget(int id)
{
printf("Returning to target %d\n", id);
state_mutex.Wait();
TargetLocation location;
int exists = 0;
location = target_manager.Get(id);
exists = target_manager.Exists(id);
state_mutex.Post();
if (!exists)
{
printf("--> No such target\n");
}
else
{
double theta = location.theta;
double phi = location.phi;
printf("--> Should go to %g %g\n", theta, phi);
RequestOrientation(theta,phi);
}
}
virtual void Body()
{
int ct = 0;
char txt[128] = "Hello there";
char reply[128] = "Not set";
YARPUniqueNameID* id = NULL;
do
{
out.Wait();
cout << "Looking up name" << endl;
cout.flush();
out.Post();
id = YARPNameService::LocateName(REG_LOCATE_NAME, NET_NAME);
if (id->getServiceType () == YARP_NO_SERVICE_AVAILABLE)
{
ACE_DEBUG ((LM_DEBUG, "can't locate name, bailing out\n"));
return;
}
id->setServiceType(YARP_TCP);
YARPEndpointManager::CreateOutputEndpoint (*id);
YARPEndpointManager::ConnectEndpoints (*id, SRC_NAME);
YARPTime::DelayInSeconds(0.2);
}
while (!id->isValid());
int x = 42;
while (!IsTerminated())
{
out.Wait();
cout << "Preparing to send: " << txt << endl;
cout.flush();
out.Post();
YARPMultipartMessage smsg(2);
smsg.Set(0,txt,sizeof(txt));
smsg.Set(1,(char*)(&x),sizeof(x));
YARPMultipartMessage rmsg(2);
double y = 999;
rmsg.Set(0,reply,sizeof(reply));
rmsg.Set(1,(char*)(&y),sizeof(y));
cout << "***sending: " << txt << endl;
cout.flush();
YARPSocketSyncComm::Send(*id,smsg,rmsg);
x++;
out.Wait();
cout << "Got reply : " << reply << " and number " << y << endl;
cout.flush();
out.Post();
YARPTime::DelayInSeconds(2.0);
ct++;
sprintf(txt, "And a-%d", ct);
}
}
virtual void Body()
{
int ct = 0;
char txt[128] = "Hello there";
char reply[128] = "Not set";
YARPUniqueNameID* id = NULL;
cout << "Here" << endl;
cout.flush();
do
{
out.Wait();
cout << "Looking up name" << endl;
cout.flush();
out.Post();
id = YARPNameService::LocateName(REG_LOCATE_NAME, NET_NAME);
if (id->getServiceType () == YARP_NO_SERVICE_AVAILABLE)
{
ACE_DEBUG ((LM_DEBUG, "can't locate name, bailing out\n"));
return;
}
YARPEndpointManager::CreateOutputEndpoint (*id);
YARPEndpointManager::ConnectEndpoints (*id, SRC_NAME);
if (!id->isValid())
{
YARPTime::DelayInSeconds(0.2);
}
}
while (!id->isValid());
while (!IsTerminated())
{
out.Wait();
cout << "Preparing to send: " << txt << endl;
cout.flush();
out.Post();
YARPSyncComm::Send(id->getNameID(),txt,sizeof(txt),reply,sizeof(reply));
out.Wait();
cout << "Got reply : " << reply << endl;
cout.flush();
out.Post();
YARPTime::DelayInSeconds(2.0);
ct++;
sprintf(txt, "And a-%d", ct);
}
}
virtual void Body()
{
char buf[128] = "Not set";
char reply[128] = "Not set";
int ct = 0;
YARPTime::DelayInSeconds(0.01);
YARPUniqueNameID* id = YARPNameService::RegisterName(REG_TEST_NAME, NET_NAME, YARP_TCP, YARP_UDP_REGPORTS);
if (id->getServiceType() == YARP_NO_SERVICE_AVAILABLE)
{
ACE_DEBUG ((LM_DEBUG, "can't register name, bailing out\n"));
return;
}
/// create the input endpoint.
YARPEndpointManager::CreateInputEndpoint (*id);
out.Wait();
///cout << "*** The assigned port is " << ((YARPUniqueNameSock *)id)->getPorts()[0] << endl;
cout.flush();
out.Post();
while (!IsTerminated())
{
out.Wait();
cout << "Waiting for input" << endl;
cout.flush();
out.Post();
YARPMultipartMessage imsg(2);
int x = 999;
imsg.Set(0,buf,sizeof(buf));
imsg.Set(1,(char*)(&x),sizeof(x));
YARPNameID reply_id = YARPSocketSyncComm::BlockingReceive(id->getNameID(),imsg);
out.Wait();
sprintf(reply,"<%s,%d>", buf, x);
cout << "Received message: " << buf << " and number " << x << endl;
cout.flush();
out.Post();
double y = 432.1;
YARPMultipartMessage omsg(2);
omsg.Set(0,reply,sizeof(reply));
omsg.Set(1,(char*)(&y),sizeof(y));
YARPSocketSyncComm::Reply(reply_id,omsg);
//YARPTime::DelayInSeconds(1.0);
ct++;
}
}
static void AddPort(YARPPort *port)
{
port_list_mutex.Wait();
port_list.push_back(port);
port_list_mutex.Post();
}
void Update(YARPImageOf<YarpPixelMono>& img,
YARPImageOf<YarpPixelBGR>& dest,
FiveBoxesInARow& out_boxes)
{
if (first)
{
prev.PeerCopy(img);
first = 0;
}
int count = 0;
int count2 = 0;
int index = 0;
mutex.Wait();
UpdateActivity();
int box_index = 0;
for (int k=0; k<FiveBoxesInARow::GetMaxBoxes(); k++)
{
out_boxes(k).valid = false;
}
for (int i=0; i<MAX_TRACKER; i++)
{
if (tracker[i].is_active)
{
count2++;
}
if (tracker[i].is_tracking)
{
count++;
int ox = tracker[i].box.cx;
int oy = tracker[i].box.cy;
int x = ox;
int y = oy;
int theta = 15;
if (oy>theta && oy<=img.GetHeight()-theta &&
ox>theta && ox<=img.GetWidth()-theta)
{
if (tracker[i].is_lagged)
{
track_tool.Apply(face_prev,img,ox,oy);
tracker[i].is_lagged = 0;
}
else
{
track_tool.Apply(prev,img,ox,oy);
}
x = track_tool.GetX();
y = track_tool.GetY();
}
int dx = x-ox;
int dy = y-oy;
if (dx!=0 || dy!=0)
{
// printf("Delta %d %d (to %d %d)\n", dx, dy, x, y);
}
tracker[i].box.brx += dx;
tracker[i].box.bry += dy;
tracker[i].box.tlx += dx;
tracker[i].box.tly += dy;
tracker[i].box.cx += dx;
tracker[i].box.cy += dy;
if (index<FiveBoxesInARow::GetMaxBoxes())
{
CBox2Send& dest2 = out_boxes(box_index);
box_index++;
Box& src = tracker[i].box;
dest2.xmin = src.tlx;
dest2.ymin = src.tly;
dest2.xmax = src.brx;
dest2.ymax = src.bry;
dest2.valid = true;
for (int i = -3; i<= 3; i++)
{
for (int j=-3; j<=3; j++)
{
if ((i+j)%2)
{
dest.SafePixel(x+j,y+i) = YarpPixelBGR(255,255,255);
}
else
{
dest.SafePixel(x+j,y+i) = YarpPixelBGR(0,0,0);
}
}
}
}
}
}
mutex.Post();
//if (count>0)
{
// printf("*** %d trackers tracking, %d active\n", count,
// count2);
}
prev.PeerCopy(img);
}
//--------------------------------------------------------------------------------------
// Class: SoundResources
// Method: (protected) _init
// Description: This is the internal class init method
//--------------------------------------------------------------------------------------
int
SoundResources::_init (const SoundOpenParameters& params)
{
//----------------------------------------------------------------------
// Initalize local parameters with the open params. It the open param
// is != 0 then the local param is updated if not the default value is
// used.
// Default configuration:
// 16-bit, 44KHz, stereo
//----------------------------------------------------------------------
if (params.m_Channels != 0) channels = params.m_Channels;
if (params.m_SamplesPerSec != 0) freqSample = params.m_SamplesPerSec;
if (params.m_BitsPerSample != 0) nBitsSample = params.m_BitsPerSample;
if (params.m_BufferLength != 0) dwBufferLength = params.m_BufferLength;
//----------------------------------------------------------------------
// Initialize the wave in
//----------------------------------------------------------------------
m_waveFormat.wFormatTag = WAVE_FORMAT_PCM;
m_waveFormat.nChannels = channels;
m_waveFormat.nSamplesPerSec = freqSample;
m_waveFormat.wBitsPerSample = nBitsSample;
m_waveFormat.nBlockAlign = m_waveFormat.nChannels * (m_waveFormat.wBitsPerSample/8);
m_waveFormat.nAvgBytesPerSec = m_waveFormat.nSamplesPerSec * m_waveFormat.nBlockAlign;
m_waveFormat.cbSize = 0;
m_err = waveInOpen(&m_WaveInHandle,
WAVE_MAPPER,
&m_waveFormat,
(DWORD)params.m_callbackthread_identifier, //Here I have to add the thread ID
(DWORD)this,
CALLBACK_THREAD);
if (m_err != MMSYSERR_NOERROR) {
printf("Can't open WAVE In Device! %d",m_err);
return(-2);
}
//----------------------------------------------------------------------
// Initialize Mixter
//----------------------------------------------------------------------
m_err = mixerOpen(&m_MixerHandle,
(DWORD)m_WaveInHandle,
0,
0,
MIXER_OBJECTF_HWAVEIN);
if (m_err != MMSYSERR_NOERROR) {
printf("yarpsounddriver: Device does not have mixer support! -- %08X\n", m_err);
}
//----------------------------------------------------------------------
// Print all the present lines in the audio interface
//----------------------------------------------------------------------
printf("yarpsounddriver: LINES PRESENT\n");
_print_dst_lines();
//----------------------------------------------------------------------
// This device should have a WAVEIN destination line. Let's get its ID so
// that we can determine what source lines are available to record from
//----------------------------------------------------------------------
m_mixerLine.cbStruct = sizeof(MIXERLINE);
m_mixerLine.dwComponentType = MIXERLINE_COMPONENTTYPE_DST_WAVEIN;
m_err = mixerGetLineInfo((HMIXEROBJ)m_MixerHandle,
&m_mixerLine,
MIXER_GETLINEINFOF_COMPONENTTYPE);
if (m_err != MMSYSERR_NOERROR) {
printf("Device does not have a WAVE recording control! -- %08X\n", m_err);
}
m_numSrc = m_mixerLine.cConnections; // Get how many source lines are available from which to record.
m_err = _select_line(MIXERLINE_COMPONENTTYPE_SRC_LINE); //select default source line
if (m_err == YARP_FAIL){ //Not line found, trying the auxiliary....
m_err = _select_line(MIXERLINE_COMPONENTTYPE_SRC_AUXILIARY);
}
if (m_err != YARP_OK)
printf("yarpsounddriver: Atention Source line not found!!!\n");
//----------------------------------------------------------------------
// Initialize the local buffers
//----------------------------------------------------------------------
_bmutex.Wait ();
_rawBuffer = new unsigned char [dwBufferLength];
ACE_ASSERT (_rawBuffer != NULL);
_bmutex.Post ();
return YARP_OK;
}
void EndObservations()
{
observing = 0;
mutex.Post();
}
int main_alt()
{
in_head.Register("/egomap/i:head");
in_img.Register("/egomap/i:img");
out_img.Register("/egomap/o:img");
out_cmd.Register("/egomap/o:cmd");
in_voice.Register("/egomap/i:cmd");
while (1)
{
JointPos joints;
in_img.Read();
state_mutex.Wait();
joints = state_joint;
CogGaze gaze;
gaze.Apply(joints);
double roll = gaze.roll_right;
double theta = gaze.theta_right;
double phi = gaze.phi_right;
//printf("DIR %g %g %g\n", theta, phi, roll);
global_theta = theta;
global_phi = phi;
global_roll = roll;
state_mutex.Post();
double z_x = gaze.z_right[0];
double z_y = gaze.z_right[1];
YARPImageOf<YarpPixelBGR> img;
img.Refer(in_img.Content());
int width = img.GetWidth();
int height = img.GetHeight();
float s = 50;
for (int i=0; i<width; i++)
{
YarpPixelBGR pix0(0,255,0);
img(i,width/2) = pix0;
}
for (int i=0; i<width; i++)
{
float s2 = (i-width/2.0);
float x = cos(roll)*s2;
float y = -sin(roll)*s2;
YarpPixelBGR pix(255,0,0);
img.SafePixel((int)(0.5+x+(width+1)/2.0),(int)(0.5+y+(width+1)/2.0)) = pix;
}
int step = 500;
for (int i=0; i<step; i++)
{
float theta = i*M_PI*2.0/step;
YarpPixelBGR pix(255,0,0);
float x = cos(theta)*s;
float y = sin(theta)*s;
//printf("%g %g %g\n", theta, x, y);
img.SafePixel(x+width/2,y+width/2) = pix;
}
for (int i=0; i<MAX_TARGETS; i++)
{
if (target_manager.Exists(i))
{
TargetLocation& loc = target_manager.Get(i);
float target_theta = loc.theta;
float target_phi = loc.phi;
float z_y = loc.phi/(M_PI/2);
float z_x = loc.theta/(M_PI/2);
//printf("Drawing circle for %g %g\n", loc.theta, loc.phi);
float x = z_x*s;
float y = z_y*s;
// YarpPixelBGR pix0(0,128,255);
// AddCircle(img,pix0,(int)x+width/2,(int)y+height/2,4);
// We now try to map back
// onto approximate retinotopic coordinates.
// current x, y, z available in gaze::x_right,y_right,z_right
double x_vis, y_vis;
int visible;
visible = gaze.Intersect(target_theta,target_phi,x_vis,y_vis,
CAMERA_SOURCE_RIGHT_WIDE);
/*
float zt[3];
zt[0] = sin(target_theta);
zt[1] = -cos(target_phi)*cos(target_theta);
zt[2] = sin(target_phi)*cos(target_theta);
float delta_theta = zt[0]*gaze.x_right[0] +
zt[1]*gaze.x_right[1] + zt[2]*gaze.x_right[2];
float delta_phi = zt[0]*gaze.y_right[0] +
zt[1]*gaze.y_right[1] + zt[2]*gaze.y_right[2];
float sanity = zt[0]*gaze.z_right[0] +
zt[1]*gaze.z_right[1] + zt[2]*gaze.z_right[2];
//float delta_theta = zt[0]; //target_theta - global_theta;
//float delta_phi = zt[1]; //target_phi - global_phi;
float factor_theta = 67; // just guessed these numbers
float factor_phi = 67; // so far, not linear in reality
float nx = delta_theta;
float ny = delta_phi;
float r = global_roll;
float sinr = sin(r);
float cosr = cos(r);
float fx = factor_theta;
float fy = factor_phi;
//delta_theta = nx*cosr - ny*sinr; // just guessed the signs here
//delta_phi = nx*sinr + ny*cosr; // so far
//delta_theta = nx*cosr - ny*sinr; // just guessed the signs here
//delta_phi = nx*sinr + ny*cosr; // so far
delta_theta *= factor_theta;
delta_phi *= factor_phi;
delta_phi *= 4.0/3.0;
float len = sqrt(delta_theta*delta_theta+delta_phi*delta_phi);
delta_theta += img.GetWidth()/2;
delta_phi += img.GetHeight()/2;
*/
int sanity = visible;
double delta_theta = x_vis;
double delta_phi = y_vis;
delta_theta -= 64;
delta_phi -= 64;
float len = sqrt(delta_theta*delta_theta+delta_phi*delta_phi);
delta_theta += 64;
delta_phi += 64;
if (sanity>0)
{
YarpPixelBGR pix1((len<50)?255:0,128,0);
AddCircle(img,pix1,(int)(delta_theta+0.5),
(int)(delta_phi+0.5),4);
}
else
{
//printf("Object occluded\n");
}
}
}
z_y = phi/(M_PI/2);
z_x = theta/(M_PI/2);
if (0)
for (int i=0; i<5; i++)
{
float x = z_x*s;
float y = z_y*s;
YarpPixelBGR pix(255,0,0);
YarpPixelBGR pix2(0,0,255);
img.SafePixel(i+x+width/2,y+width/2) = pix;
img.SafePixel(-i+x+width/2,y+width/2) = pix;
img.SafePixel(i+x+width/2,y+width/2-1) = pix2;
img.SafePixel(-i+x+width/2,y+width/2+1) = pix2;
img.SafePixel(x+width/2,i+y+width/2) = pix;
img.SafePixel(x+width/2,-i+y+width/2) = pix;
img.SafePixel(x+width/2+1,i+y+width/2) = pix2;
img.SafePixel(x+width/2-1,-i+y+width/2) = pix2;
}
out_img.Content().PeerCopy(in_img.Content());
out_img.Write();
}
return 0;
}
