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