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 ); } }