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;
}
void StatisticsCASARegion::_getStatsFromCalculator( casa::ImageInterface<casa::Float>* image,
const casa::Record& region, const std::vector<int>& slice,
QList<Carta::Lib::StatInfo>& stats, const QString& regionType ){
//If the region record is empty, there are not stats - just return.
int fieldCount = region.nfields();
if ( fieldCount == 0 ){
return;
}
std::shared_ptr<const casa::ImageInterface<casa::Float> > imagePtr( image->cloneII() );
casa::CoordinateSystem cs = image->coordinates();
casa::Vector<casa::Int> displayAxes = cs.directionAxesNumbers();
casa::Quantum<casa::Double> pix0( 0, "pix");
casa::IPosition shape = image->shape();
int nAxes = shape.nelements();
casa::Vector<casa::Quantum<casa::Double> > blcq( nAxes, pix0);
casa::Vector<casa::Quantum<casa::Double> > trcq( nAxes, pix0);
for ( int i = 0; i < nAxes; i++ ){
if ( i == displayAxes[0] || i == displayAxes[1]){
trcq[i].setValue( shape[i] );
}
else {
blcq[i].setValue( slice[i] );
trcq[i].setValue( slice[i] );
}
}
casa::WCBox box( blcq, trcq, cs, casa::Vector<casa::Int>());
casa::ImageRegion* imgBox = new casa::ImageRegion( box );
std::shared_ptr<casa::SubImage<casa::Float> > boxImage( new casa::SubImage<Float>(*image, *imgBox ) );
ImageStatsCalculator calc( boxImage, ®ion, "", true);
calc.setList(False);
Record result = calc.calculate();
const casa::String blcKey( "blc");
const casa::String trcKey( "trc");
_insertScalar( result, "npts", Carta::Lib::StatInfo::StatType::FrameCount, stats );
_insertScalar( result, "sum", Carta::Lib::StatInfo::StatType::Sum, stats );
_insertScalar( result, "sumsq", Carta::Lib::StatInfo::StatType::SumSq, stats );
_insertScalar( result, "min", Carta::Lib::StatInfo::StatType::Min, stats );
_insertScalar( result, "max", Carta::Lib::StatInfo::StatType::Max, stats );
_insertScalar( result, "mean", Carta::Lib::StatInfo::StatType::Mean, stats );
_insertScalar( result, "sigma", Carta::Lib::StatInfo::StatType::Sigma, stats );
_insertScalar( result, "rms", Carta::Lib::StatInfo::StatType::RMS, stats );
_insertScalar( result, "flux", Carta::Lib::StatInfo::StatType::FluxDensity, stats );
_insertList( result, blcKey, Carta::Lib::StatInfo::StatType::Blc, stats );
_insertList( result, trcKey, Carta::Lib::StatInfo::StatType::Trc, stats );
_insertList( result, "minpos", Carta::Lib::StatInfo::StatType::MinPos, stats );
_insertList( result, "maxpos", Carta::Lib::StatInfo::StatType::MaxPos, stats );
_insertString( result, "blcf", Carta::Lib::StatInfo::StatType::Blcf, stats );
_insertString( result, "trcf", Carta::Lib::StatInfo::StatType::Trcf, stats );
_insertString( result, "minposf", Carta::Lib::StatInfo::StatType::MinPosf, stats );
_insertString( result, "maxposf", Carta::Lib::StatInfo::StatType::MaxPosf, stats );
//Put in an identifier.
if ( result.isDefined( blcKey ) && result.isDefined( trcKey ) ){
casa::Vector<int> blcArray = result.asArrayInt( blcKey );
QString blcVal = _vectorToString( blcArray );
casa::Vector<int> trcArray = result.asArrayInt( trcKey );
QString trcVal = _vectorToString( trcArray );
QString idVal = regionType + ":" + blcVal;
if ( blcVal != trcVal ){
idVal = idVal + " x " + trcVal;
}
Carta::Lib::StatInfo info( Carta::Lib::StatInfo::StatType::Name );
info.setValue( idVal );
info.setImageStat( false );
stats.append( info );
}
}
