📄 locwm.cc
字号:
M->KFSelfDistance(beaconCoords[visionObjects_[i].type_][0], beaconCoords[visionObjects_[i].type_][1],visionObjects_[i].distance_,i,sdDistance);
M->KFSelfHeading(beaconCoords[visionObjects_[i].type_][0], beaconCoords[visionObjects_[i].type_][1],visionObjects_[i].heading_,visionObjects_[i].distance_,i, sdAngle);
}
//goalposts
if ((visionType[i]>=VisionObject:: OT_GOAL_B_L)&&(visionType[i]<=VisionObject::OT_GOAL_Y_R))
{
GoalPost(M,i);
}
//outside corners
if ((visionType[i]==VisionObject::OT_GOAL_CORNER_OUT)&&goalKeeper)
{
//outsideCorners++;
}
//if dog sees inside corner decide if we can be sure which corner
//only works for goal keeper at this stage
if (visionType[i]==VisionObject::OT_GOAL_CORNER_IN)
{
M->GoalCornerIn(i,beaconSeen,sdAngle);
}
//centre point
if(visionType[i]==VisionObject::OT_CENTRE_POINT)
{
M->KFSelfHeading(0,0,visionObjects_[i].heading_,visionObjects_[i].distance_,i,sdAngle);
//cout<<"Centre point being used"<<endl<<flush;
}
//points below green beacons on centre line
if(visionType[i]==VisionObject::OT_CENTRE_LINE_CORNER)
{
M->CentreLineCorner(i,sdAngle);
}
//ball
if (visionType[i]==VisionObject::OT_BALL)
{
// CM
#ifndef NO_KF_BALL
M->KFBall(visionObjects_[i].heading_,visionObjects_[i].distance_,i);
#endif
for (int j=0; j<6; j++)
{
if(beaconSeen[j]==1)
{
// CM
#ifndef NO_KF_BALL
// CM did this, on Rick's advice.
// int index=j+2;
// M->KFBallObject(beaconCoords[index][0], beaconCoords[index][1], visionObjects_[beaconIndex[j]].heading_, visionObjects_[i].heading_);
#endif
}
}
}
}
M->RobotPosnClip();
M->BallPosnClip();
if(currentModel==modelNo)
{
worldObjects_[0].x_ = M->X[0][0];
worldObjects_[0].y_ = M->X[1][0];
worldObjects_[0].heading_ = Normalise_PI(M->X[2][0]);
worldObjects_[0].uncertX_ = sqrt(M->P[0][0]);
worldObjects_[0].uncertY_ = sqrt(M->P[1][1]);
worldObjects_[0].uncertHeading_ = sqrt(M->P[2][2]);
//Own location for World Model program
worldObjectsDebug_[BOTID_-1].x_ = M->X[0][0];
worldObjectsDebug_[BOTID_-1].y_ = M->X[1][0];
worldObjectsDebug_[BOTID_-1].heading_ = Normalise_PI(M->X[2][0]);
worldObjectsDebug_[BOTID_-1].uncertX_ = sqrt(M->P[0][0]);
worldObjectsDebug_[BOTID_-1].uncertY_ = sqrt(M->P[1][1]);
worldObjectsDebug_[BOTID_-1].uncertHeading_ = sqrt(M->P[2][2]);
//Ball location for World Model program
worldObjectsDebug_[5].object_ = WorldObject::WM_OT_BALL_IND;
worldObjectsDebug_[5].x_ =M->X[3][0] ;
worldObjectsDebug_[5].y_ =M->X[4][0];
worldObjectsDebug_[5].heading_ = 0;
worldObjectsDebug_[5].uncertX_ =sqrt(M->P[3][3]);
worldObjectsDebug_[5].uncertY_ =sqrt(M->P[4][4]);
worldObjectsDebug_[5].uncertHeading_ = 0;
}
}
void LOCWM::GoalPost(KF* M, int i)
{
if (robotState_.GetTeam() == RobotState::RT_BLUE)
{
if ((visionObjects_[i].type_>=VisionObject:: OT_GOAL_B_L)&&(visionObjects_[i].type_<=VisionObject::OT_GOAL_B_R))
{
if (postAngleUpdate)
{
M->KFSelfHeading(beaconCoords[visionObjects_[i].type_][0], beaconCoords[visionObjects_[i].type_][1],visionObjects_[i].heading_,visionObjects_[i].distance_,i,sdAngle);
}
if (postDistUpdate)
{
M->KFSelfDistance(beaconCoords[visionObjects_[i].type_][0], beaconCoords[visionObjects_[i].type_][1],visionObjects_[i].distance_,i,sdDistance);
}
}
if ((visionObjects_[i].type_>=VisionObject:: OT_GOAL_Y_L)&&(visionObjects_[i].type_<=VisionObject::OT_GOAL_Y_R))
{
M->KFSelfDistance(beaconCoords[visionObjects_[i].type_][0], beaconCoords[visionObjects_[i].type_][1],visionObjects_[i].distance_,i,sdDistance);
M->KFSelfHeading(beaconCoords[visionObjects_[i].type_][0], beaconCoords[visionObjects_[i].type_][1],visionObjects_[i].heading_,visionObjects_[i].distance_,i,sdAngle);
}
}
if (robotState_.GetTeam() == RobotState::RT_RED)
{
if ((visionObjects_[i].type_>=VisionObject:: OT_GOAL_Y_L)&&(visionObjects_[i].type_<=VisionObject::OT_GOAL_Y_R))
{
if (postAngleUpdate)
{
M->KFSelfHeading(beaconCoords[visionObjects_[i].type_][0], beaconCoords[visionObjects_[i].type_][1],visionObjects_[i].heading_,visionObjects_[i].distance_,i,sdAngle);
}
if (postDistUpdate)
{
M->KFSelfDistance(beaconCoords[visionObjects_[i].type_][0], beaconCoords[visionObjects_[i].type_][1],visionObjects_[i].distance_,i,sdDistance);
}
}
if ((visionObjects_[i].type_>=VisionObject:: OT_GOAL_B_L)&&(visionObjects_[i].type_<=VisionObject::OT_GOAL_B_R))
{
M->KFSelfDistance(beaconCoords[visionObjects_[i].type_][0], beaconCoords[visionObjects_[i].type_][1],visionObjects_[i].distance_,i,sdDistance);
M->KFSelfHeading(beaconCoords[visionObjects_[i].type_][0], beaconCoords[visionObjects_[i].type_][1],visionObjects_[i].heading_,visionObjects_[i].distance_,i,sdAngle);
}
}
}
void LOCWM::BeaconGoalPost(KF* M)
{
if(postSeen[0]&&beaconSeen[2])//left blue post and blue-pink beacon
{
if (robotState_.GetTeam() == RobotState::RT_RED || (postDistUpdate && postAngleUpdate)) {
TwoObjects(M,beaconIndex[2],postIndex[0]);
//Angles and other distance are discarded
visionType[postIndex[0]]=VisionObject::OT_INVALID;
visionType[beaconIndex[2]]=VisionObject::OT_INVALID;
}
}
if(postSeen[1]&&beaconSeen[5])//right blue post and pink-blue beacon
{
if (robotState_.GetTeam() == RobotState::RT_RED || (postDistUpdate && postAngleUpdate)) {
TwoObjects(M,beaconIndex[5],postIndex[1]);
//Angles and other distance are discarded
visionType[postIndex[1]]=VisionObject::OT_INVALID;
visionType[beaconIndex[5]]=VisionObject::OT_INVALID;
}
}
if(postSeen[2]&&beaconSeen[3])//left yellow post and yellow-pink beacon
{
if (robotState_.GetTeam() == RobotState::RT_BLUE || (postDistUpdate && postAngleUpdate)) {
TwoObjects(M,beaconIndex[3],postIndex[2]);
//Angles and other distance are discarded
visionType[postIndex[2]]=VisionObject::OT_INVALID;
visionType[beaconIndex[3]]=VisionObject::OT_INVALID;
}
}
if(postSeen[3]&&beaconSeen[0])//right yellow post and pink-yellow beacon
{
if (robotState_.GetTeam() == RobotState::RT_BLUE || (postDistUpdate && postAngleUpdate)) {
TwoObjects(M,beaconIndex[0],postIndex[3]);
//Angles and other distance are discarded
visionType[postIndex[3]]=VisionObject::OT_INVALID;
visionType[beaconIndex[0]]=VisionObject::OT_INVALID;
}
}
}
void LOCWM::TwoObjects(KF* M, int beaconIndex,int postIndex)
{
double markerdist=0;
double markerangle=0;
markerdist=115;
markerangle=visionObjects_[postIndex].heading_-visionObjects_[beaconIndex].heading_;
M->KFTwoObjects(markerdist,ABS(markerangle),beaconCoords[visionObjects_[beaconIndex].type_][0], beaconCoords[visionObjects_[beaconIndex].type_][1],beaconCoords[visionObjects_[postIndex].type_][0], beaconCoords[visionObjects_[postIndex].type_][1]);
//distance to beacon automatically used
//(closest object is chosen and distance to object used for measurement update) no longer correct
/*if(visionObjects_[postIndex].distance_ < visionObjects_[beaconIndex].distance_)
{
M1.KFSelfDistance(beaconCoords[visionObjects_[postIndex].type_][0], beaconCoords[visionObjects_[postIndex].type_][1],visionObjects_[postIndex].distance_,postIndex,sdDistance);
}*/
//else
//{
// changed by CM. Rick's suggestion
// M->KFSelfDistance(beaconCoords[visionObjects_[beaconIndex].type_][0], beaconCoords[visionObjects_[beaconIndex].type_][1],visionObjects_[beaconIndex].distance_, beaconIndex,sdDistance);
M->KFSelfHeading(beaconCoords[visionObjects_[beaconIndex].type_][0], beaconCoords[visionObjects_[beaconIndex].type_][1],visionObjects_[beaconIndex].heading_,visionObjects_[beaconIndex].distance_, beaconIndex,sdAngle);
//delme M->KFSelfHeading(beaconCoords[visionObjects_[i].type_][0], beaconCoords[visionObjects_[i].type_][1],visionObjects_[i].heading_,visionObjects_[i].distance_,i, sdAngle);
//M2.KFSelfDistance(beaconCoords[visionObjects_[beaconIndex].type_][0], beaconCoords[visionObjects_[beaconIndex].type_][1],visionObjects_[beaconIndex].distance_, beaconIndex,sdDistance);
//}
}
void LOCWM::BallPosition() {
//This implements a very simple ball positioning strategy based on geometry
for (int i = 0; i < numVisionObjects_; i++) {
if (visionObjects_[i].type_==VisionObject::OT_BALL) {
//Calculate ball's absolute heading
static double ballAbsHeading,ballx,bally;
ballAbsHeading = Normalise_PI(visionObjects_[i].heading_ + worldObjects_[0].heading_);
//Calculate absolute position of ball.
ballx=worldObjects_[0].x_ - ABS(visionObjects_[i].distance_)*sin(ballAbsHeading);
bally= worldObjects_[0].y_ + ABS(visionObjects_[i].distance_)*cos(ballAbsHeading);
//Robot is out of x-axis
if (ballx>MAXXCOORD) {
//Assume the angle is correct and the distance is wrong
ballx=MAXXCOORD;
bally=worldObjects_[0].y_ + ABS((MAXXCOORD-worldObjects_[0].x_)/sin(ballAbsHeading))*cos(ballAbsHeading);
} else if (ballx<MINXCOORD) {
ballx=MINXCOORD;
bally=worldObjects_[0].y_ + ABS((MINXCOORD-worldObjects_[0].x_)/sin(ballAbsHeading))*cos(ballAbsHeading);
}
//Robot is out of y-axis
if (bally>MAXYCOORD) {
//Assume the angle is correct and the distance is wrong
ballx=worldObjects_[0].x_ - ABS((MAXYCOORD-worldObjects_[0].y_)/cos(ballAbsHeading))*sin(ballAbsHeading);
bally=MAXYCOORD;
} else if (bally<MINYCOORD) {
ballx=worldObjects_[0].x_ - ABS((MINYCOORD-worldObjects_[0].y_)/cos(ballAbsHeading))*sin(ballAbsHeading);
bally=MINYCOORD;
}
static int index;
index = numWorldObjects_;
numWorldObjects_++;
worldObjects_[index].objSeenThisFrame_=true;
worldObjects_[index].object_ = WorldObject::WM_OT_BALL_IND;
worldObjects_[index].x_ = ballx;
worldObjects_[index].y_ = bally;
worldObjects_[index].heading_ = 0;
//For the moment assume that there is no error in the angle of the ball.
//Divide the error of the ball distance into x and y components.
worldObjects_[index].uncertX_ = worldObjects_[0].uncertX_+ABS(visionObjects_[i].distance_/3*sin(ballAbsHeading));
worldObjects_[index].uncertY_ = worldObjects_[0].uncertY_+ABS(visionObjects_[i].distance_/3*cos(ballAbsHeading));
worldObjects_[index].uncertHeading_ = 0;
}
}
}
void LOCWM::SetDataForTeam(WorldObject *objs, int num)
{
if (robotState_.GetTeam() != RobotState::RT_BLUE)
return;
for (int i = 0; i < num; i++) {
objs[i].x_ = -1 * objs[i].x_;
objs[i].y_ = -1 * objs[i].y_;
objs[i].heading_ = Normalise_PI(PI + objs[i].heading_);
}
}
void LOCWM::Restart()
{
currentModel=2;
noFramesBallSeen=0;
M1.Restart();
// M2.Restart();
}
void LOCWM::LoadParameters(char* parameterFile) {
configuration_.ParseFile(parameterFile); // Configuration is of type parse
// changing these parameters changes balances between odometry and measurements. see kf.cc for details.
distErrorPerFrameStill = configuration_.GetAsDouble("distErrorStill")/30.0;
angleErrorPerFrameStill = configuration_.GetAsDouble("angleErrorStill")/30.0;
distErrorPerFrameMoving = configuration_.GetAsDouble("distErrorMoving")/30.0;
angleErrorPerFrameMoving = configuration_.GetAsDouble("angleErrorMoving")/30.0;
ballDistErrorPerFrame = configuration_.GetAsDouble("ballDistErrorMoving")/30.0;
ballDistErrorPerFrameStill = configuration_.GetAsDouble("ballDistErrorStill")/30.0;
postDistUpdate =configuration_.GetAsBool("goalPostDist");
postAngleUpdate =configuration_.GetAsBool("goalPostAngle");
cornerAngleUpdate =configuration_.GetAsBool("useCorners");
goalKeeper = configuration_.GetAsBool("goalKeeper");
sdDistance = configuration_.GetAsDouble("sdDist");
sdAngle = configuration_.GetAsDouble("sdAngle");
delta = configuration_.GetAsDouble("delta");
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -