void KfieldGui::draw_ball(belief Belief, BallObject Ball) { pthread_mutex_lock(&lock); CvPoint pt1, pt2; int radius = Ball.ball_diameter() / 2 * 1000.0 / scale; //double max = 0; pt1.x = Belief.x + Ball.dist() * 1000 * cos((Belief.theta + Ball.bearing())); pt1.y = Belief.y + Ball.dist() * 1000 * sin((Belief.theta + Ball.bearing())); pt2.x = (pt1.x + (2 * margintoline + field_width) / 2.0) / scale; pt2.y = (-pt1.y + (2 * margintoline + field_height) / 2.0) / scale; cout << "Ball Dist" << Ball.dist() << " Ball diameter " << Ball.ball_diameter() << " Bearing " << Ball.bearing() << " pt x:" << pt1.x << " y: " << pt1.y << endl; cout << " Points in the field pt2.x " << pt2.x << " pt2.y " << pt2.y << endl; cvCopy(cleanfield, field); cvCircle(field, pt2, radius, color["orange"], CV_FILLED, CV_AA, 0); tmp = Kutils::to_string(Ball.dist()); cvPutText(field, tmp.c_str(), cvPoint((2 * margintoline + field_width - 400) / scale, (2 * margintoline + field_height + 850) / scale), &font, color["orange"]); pthread_mutex_unlock(&lock); //cout << " Ball Drawn " << endl; }
void Vision::fetchAndProcess() { leds.Clear(); obs.Clear(); vdm.Clear(); //unsigned long startt = SysCall::_GetCurrentTimeInUSec(); KSystem::Time::TimeAbsolute oldstamp = stamp; boost::shared_ptr<const KRawImage> img = _blk.readData<KRawImage> ("image", Messaging::MessageEntry::HOST_ID_LOCAL_HOST, &stamp); if(stamp <= oldstamp) return ; if(img.get() == 0) return; // cout << "haveimage" << endl; //Remove constness, tricky stuff :/ rawImage.copyFrom(img->image_rawdata().data(), img->width(), img->height(), img->bytes_per_pix()); obs.set_image_timestamp(stamp.toString()); vdm.set_image_timestamp(stamp.toString()); //unsigned long endt = SysCall::_GetCurrentTimeInUSec()-startt; //cout<<"Fetch image takes:"<<endt<<endl; stamp += KSystem::Time::TimeAbsolute::milliseconds(config.sensordelay); if (img->active_camera() == KRawImage::BOTTOM)//bottom cam { //Get Kinematics first! std::vector<float> val = kinext.getKinematics("CameraBottom"); p.cameraPitch = (KMath::KMat::transformations::PI * 40.0) / 180.0; //cout<<"CameraPitch:"<<cameraPitch<<endl; seg = segbottom; p.cameraX = val[0]; p.cameraY = val[1]; p.cameraZ = val[2];//3rd element } else { //Get Kinematics first! std::vector<float> val = kinext.getKinematics("CameraTop"); p.cameraX = val[0]; p.cameraY = val[1]; p.cameraZ = val[2];//3rd element p.cameraPitch = 0; seg = segtop; } seg->setLumaScale(pow(img->luminance_scale(), config.cameraGamma) ); //std::cout<<img->luminance_scale()<<std::endl; //cout<<"Attach to Image:"<<seg<<rawImage<<endl; seg->attachToIplImage(rawImage);//Make segmentator aware of a new image //saveFrame(rawImage); //return; asvmo = _blk.readData<AllSensorValuesMessage> ("sensors", Messaging::MessageEntry::HOST_ID_LOCAL_HOST, &timeo, &stamp, Blackboard::DATA_NEAREST_NOTNEWER); asvmn = _blk.readData<AllSensorValuesMessage> ("sensors", Messaging::MessageEntry::HOST_ID_LOCAL_HOST, &timen, &stamp, Blackboard::DATA_NEAREST_NOTOLDER); #ifdef DEBUGVISION cout << "ImageTimestamp:" << KSystem::Time::to_iso_string(stamp) << endl; cout << "Now:" << KSystem::Time::SystemTime::now() << endl; cout << "SensorTimestamp:" << KSystem::Time::to_iso_string(timeo) << "," << KSystem::Time::to_iso_string(timen) << endl; // KSystem::Time::TimeAbsolute t; // _blk.readData<AllSensorValuesMessage> ("sensors", Messaging::MessageEntry::HOST_ID_LOCAL_HOST, &t); // cout << "Lasttimestamp:"<< KSystem::Time::to_iso_string(t) << endl; #endif if (asvmn.get() == NULL || asvmo.get() == NULL) //No sensor data! { LogEntry(LogLevel::Error,GetName()) << "Warning!!! Vision has no head joint msg in all sensor (allsm) data!"; return; } float imcomp = ( (float ) ((stamp - timeo).raw()) ) / KSystem::Time::TimeAbsolute::TPS; assert(imcomp >= 0); //each value is (n-o)*(stamp-timeo) +o p.yaw = asvmo->jointdata(KDeviceLists::HEAD + KDeviceLists::YAW).sensorvalue(); p.yaw += (asvmn->jointdata(KDeviceLists::HEAD + KDeviceLists::YAW).sensorvalue() - p.yaw) * imcomp; p.pitch = asvmo->jointdata(KDeviceLists::HEAD + KDeviceLists::PITCH).sensorvalue(); p.pitch += (asvmn->jointdata(KDeviceLists::HEAD + KDeviceLists::PITCH).sensorvalue() - p.pitch) * imcomp; //p.Vyaw = asvmo->jointdata(KDeviceLists::HEAD+KDeviceLists::YAW).sensorvaluediff(); //p.Vpitch = asvmo->jointdata(KDeviceLists::HEAD+KDeviceLists::PITCH).sensorvaluediff(); p.angX = asvmo->computeddata(KDeviceLists::ANGLE + KDeviceLists::AXIS_X).sensorvalue(); p.angX += (asvmn->jointdata(KDeviceLists::HEAD + KDeviceLists::AXIS_X).sensorvalue() - p.angX) * imcomp; p.angX += config.rolloffset; p.angY = asvmo->computeddata(KDeviceLists::ANGLE + KDeviceLists::AXIS_Y).sensorvalue(); p.angY += (asvmn->jointdata(KDeviceLists::HEAD + KDeviceLists::AXIS_Y).sensorvalue() - p.angY) * imcomp; p.angY += config.pitchoffset; //p.VangX = asvm->computeddata(KDeviceLists::ANGLE+KDeviceLists::AXIS_X).sensorvaluediff(); //p.VangY = asvm->computeddata(KDeviceLists::ANGLE+KDeviceLists::AXIS_Y).sensorvaluediff(); //p.timediff = asvm->timediff();//Get time from headmessage #ifdef DEBUGVISION cout << p.yaw << " " << p.pitch << " " << p.angX << " " << p.angY << imcomp << endl; #endif p.focallength = sqrt(sqrd(rawImage.width) + sqrd(rawImage.height) ) / (2 * tan(config.Dfov * TO_RAD / 2)); //Logger::Instance().WriteMsg("Vision", _toString("Focal Length ")+_toString(p.focallength), Logger::Error); kinext.setPose(p); KMath::KMat::transformations::makeRotation(simpleRot, -kinext.getRoll()); //Now change y axis :) simpleRot(0, 1) = -simpleRot(0, 1); simpleRot(1, 1) = -simpleRot(1, 1); //Weird stuff. simpleRot-^-1=simpleRot'=simpleRot. therefore translaton from raw image to rotated and axis inverted //image and back is done by simpleRot // // Vup.y = -cos(-kinext.getRoll()); // Vup.x = sin(-kinext.getRoll()); Vup = simpleRot.slow_mult(KVecFloat2(0, 1)); Vdn = simpleRot.slow_mult(KVecFloat2(0, -1)); Vlt = simpleRot.slow_mult(KVecFloat2(-1, 0)); Vrt = simpleRot.slow_mult(KVecFloat2(1, 0)); KVecFloat2 c2d; KVecFloat3 c3d; //Publish the projection of the camera limits on the ground PointObject *p1; c2d = imageToCamera(KVecFloat2(0, 0)); c3d = kinext.camera2dToGround(c2d); if(c3d(2) < 0) //Not Looking up :) { p1 = obs.add_view_limit_points(); measurement *m = kinext.projectionDistance(c2d, 0); p1->set_distance(m[0].mean); p1->set_bearing(m[1].mean); //cout<<"See bot:"<<newpost.distBot.mean<<endl; delete[] m; } c2d = imageToCamera(KVecFloat2(0, rawImage.height)); c3d = kinext.camera2dToGround(c2d); if(c3d(2) < 0) //Not Looking up :) { p1 = obs.add_view_limit_points(); measurement *m = kinext.projectionDistance(c2d, 0); p1->set_distance(m[0].mean); p1->set_bearing(m[1].mean); //cout<<"See bot:"<<newpost.distBot.mean<<endl; delete[] m; } c2d = imageToCamera(KVecFloat2(rawImage.width, rawImage.height)); c3d = kinext.camera2dToGround(c2d); if(c3d(2) < 0) //Not Looking up :) { p1 = obs.add_view_limit_points(); measurement *m = kinext.projectionDistance(c2d, 0); p1->set_distance(m[0].mean); p1->set_bearing(m[1].mean); //cout<<"See bot:"<<newpost.distBot.mean<<endl; delete[] m; } c2d = imageToCamera(KVecFloat2(rawImage.width, 0)); c3d = kinext.camera2dToGround(c2d); if(c3d(2) < 0) //Not Looking up :) { p1 = obs.add_view_limit_points(); measurement *m = kinext.projectionDistance(c2d, 0); p1->set_distance(m[0].mean); p1->set_bearing(m[1].mean); //cout<<"See bot:"<<newpost.distBot.mean<<endl; delete[] m; } /*Vdn.x = -Vup.x; Vdn.y = -Vup.y; Vlt.x = Vup.y;//-Vup.x*cos(-45*TO_RAD)-Vup.y*sin(-45*TO_RAD); Vlt.y = -Vup.x;//-Vup.x*sin(-45*TO_RAD)+Vup.y*sin(-45*TO_RAD); Vrt.x = -Vup.y; Vrt.y = Vup.x;*/ //unsigned long startt,endt; //startt=SysCall::_GetCurrentTimeInUSec(); ballpixels.clear(); ygoalpost.clear(); obstacles.clear(); tobeshown.clear(); gridScan(orange); //endt = SysCall::_GetCurrentTimeInUSec()-startt; //cout<<"GridScan takes:"<<endt<<endl; //startt=SysCall::_GetCurrentTimeInUSec(); balldata_t b = locateBall(ballpixels); //endt = SysCall::_GetCurrentTimeInUSec()-startt; //cout<<"locateball takes:"<<endt<<endl; //unsigned long endt = SysCall::_GetCurrentTimeInUSec()-startt; //cout<<"locateball takes:"<<endt<<endl; //cout<<b.r<<endl; locateGoalPost(ygoalpost, yellow); #ifdef DEBUGVISION cout << "Ballpixelsize:" << ballpixels.size() << endl; //cout << b.x << " " << b.y << " " << b.cr << endl; //KVecFloat2 & w=camToRobot(o) cout << "Bearing:" << b.bearing.mean << " " << b.bearing.var << endl; cout << "Distance:" << b.distance.mean << " " << b.distance.var << endl; #endif if (b.cr > 0) { //Fill message and publish KVecFloat2 im; im(0) = b.x; im(1) = b.y; KVecFloat2 c; KVecFloat3 c3d; c = imageToCamera(im); c3d = kinext.camera2dToTorso(c); //c3d(0)+p.cameraX;c3d(1)+cameraY; //if(sqrt(sqrd(c3d(0)) + sqrd(c3d(1)))<0.1) // c3d.scalar_mult(2); float pitch, yaw; pitch = atan(abs(c3d(2)) / sqrt(sqrd(c3d(0)) + sqrd(c3d(1)))); yaw = atan2(c3d(1), c3d(0)); float w = b.distance.mean * 3; w = w > 1 ? 1 : w; BallObject ballpos; ballpos.set_dist(b.distance.mean); ballpos.set_bearing(b.bearing.mean); ballpos.set_ball_diameter(b.ballradius * 2); obs.mutable_ball()->CopyFrom(ballpos); LedValues* l = leds.add_leds(); l->set_chain("l_eye"); l->set_color("red"); } else { LedValues* l = leds.add_leds(); l->set_chain("l_eye"); l->set_color("white"); } if (obs.regular_objects_size() > 0) { bool yellow = false; ::google::protobuf::RepeatedPtrField<const ::NamedObject>::const_iterator ptr = obs.regular_objects().begin(); while (ptr != obs.regular_objects().end()) { if ((*ptr).object_name().c_str()[0] == 'Y') yellow = true; ++ptr; } LedValues* l = leds.add_leds(); l->set_chain("r_eye"); if (obs.regular_objects_size() == 1 && obs.regular_objects(0).object_name().compare("Yellow")==0) { l->set_color("blue"); }else if(obs.regular_objects_size() == 1){ l->set_color("yellow"); }else if (obs.regular_objects_size() == 2) { l->set_color("purple"); } } else { LedValues* l = leds.add_leds(); l->set_chain("r_eye"); l->set_color("black"); } _blk.publishSignal(vdm, "debug"); _blk.publishSignal(leds, "leds"); _blk.publishSignal(obs, "vision"); }
QRectF KFieldScene::visionBallRect (BallObject bob, WorldInfo wim) { float xmiddle = (wim.myposition().x() + bob.dist() * cos ( (wim.myposition().phi() + bob.bearing() ) ) ) * 1000; float ymiddle = (wim.myposition().y() + bob.dist() * sin ( (wim.myposition().phi() + bob.bearing() ) ) ) * 1000; return rectFromFC ( xmiddle, ymiddle, 75, 75); }
void LocalWorldState::calculateBallEstimate(Localization::KMotionModel const & robotModel) { KSystem::Time::TimeDuration duration; KSystem::Time::TimeAbsolute observationTime; //float dt; float dx,dy,gx,gy; bool ballseen = false; //cout << " Ball position estimation " << endl; if (obsm.get() && obsm->has_ball()) { //cout << " Ball observation " << endl; //used for removing the ball if exceeds a threshold BallObject aball = obsm->ball(); dx = aball.dist() * cos(aball.bearing()); dy = aball.dist() * sin(aball.bearing()); gx = AgentPosition.x + dx * cos(AgentPosition.phi) + dy * sin(AgentPosition.phi); gy = AgentPosition.y - dx * sin(AgentPosition.phi) + dy * cos(AgentPosition.phi); if ( fabs(gx) < locConfig.fieldMaxX + 2 && fabs(gy) < locConfig.fieldMaxY + 2) { ballseen = true; observationTime = now; if (MyWorld.balls_size() < 1) { //cout << " Inserting new ball " << endl; MyWorld.add_balls(); myBall.reset(aball.dist(), 0, aball.bearing(), 0); ballTimeReset = 1; lastObservationTime = now; } else { if (ballTimeReset < MAX_TIME_TO_RESET) ballTimeReset+= 0.5; //Predict //cout << " Ball exists.. Predict + update " << endl; duration = observationTime - lastObservationTime; lastObservationTime = now; if (duration > KSystem::Time::TimeAbsolute::seconds(MAX_TIME_TO_RESET)) { myBall.reset(aball.dist(), 0, aball.bearing(), 0); } else{ duration = observationTime - lastFilterTime; // dt = duration.total_microseconds() / 1000000.0f; myBall.update(aball.dist(), 0.25 , aball.bearing(), 0.03); myBall.predict(duration.toFloat(),robotModel); } } lastFilterTime = now; //cout << " Sending ball position and velocity " << endl; //Create message MyWorld.mutable_balls(0)->set_relativex(myBall.state(0,0)); MyWorld.mutable_balls(0)->set_relativey(myBall.state(1,0)); MyWorld.mutable_balls(0)->set_relativexspeed(myBall.state(2,0)); MyWorld.mutable_balls(0)->set_relativeyspeed(myBall.state(3,0)); } } if (!ballseen) { //cout << " ball is not being seen " << endl; duration = now - lastObservationTime; //dt = duration.total_microseconds() / 1000000.0f; if (duration.toFloat() > ballTimeReset) { if (MyWorld.balls_size() > 0){ ballTimeReset = 0; //cout << " Reseting ball " << endl; myBall.reset(0, 0, 0, 0); MyWorld.clear_balls(); } } else { duration = now - lastFilterTime; //dt = duration.total_microseconds() / 1000000.0f; if (MyWorld.balls_size() > 0){ //cout << " ball is not being seen .. predict movement" << endl; myBall.predict(duration.toFloat(),robotModel); MyWorld.mutable_balls(0)->set_relativex(myBall.state(0,0)); MyWorld.mutable_balls(0)->set_relativey(myBall.state(1,0)); MyWorld.mutable_balls(0)->set_relativexspeed(myBall.state(2,0)); MyWorld.mutable_balls(0)->set_relativeyspeed(myBall.state(3,0)); } } lastFilterTime = now; } //myBall.state.prettyPrint(); }