/*! @brief Stream insertion operator for a single ColourSegment.
* The segment is terminated by a newline.
*/
ostream& operator<< (ostream& output, const Goal& g)
{
output << "Goal - " << VFOName(g.m_id) << endl;
output << "\tpixelloc: " << g.m_location.screen << endl;
output << "\tangularloc: " << g.m_location.angular << endl;
output << "\trelative field coords: " << g.m_location.relativeRadial << endl;
output << "\tspherical error: " << g.m_spherical_error << endl;
output << "\tsize on screen: " << g.m_size_on_screen;
return output;
}
bool CornerPoint::addToExternalFieldObjects(FieldObjects* fieldobjects, float timestamp) const
{
#if VISION_FIELDPOINT_VERBOSITY > 1
debug << "CornerPoint::addToExternalFieldObjects - m_id: " << VFOName(m_id) << endl;
debug << " " << *this << endl;
#endif
if(valid) {
#if VISION_FIELDPOINT_VERBOSITY > 1
debug << "CornerPoint::addToExternalFieldObjects - valid" << endl;
#endif
AmbiguousObject newAmbObj;
switch(m_type) {
case L:
// labelling inside L since I have no idea which is which - going to match it with other possibilities
newAmbObj = AmbiguousObject(FieldObjects::FO_CORNER_UNKNOWN_INSIDE_L, "L Corner");
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_YELLOW_FIELD_LEFT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_YELLOW_FIELD_RIGHT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_YELLOW_PEN_LEFT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_YELLOW_PEN_RIGHT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_CENTRE_CIRCLE);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_BLUE_PEN_LEFT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_BLUE_PEN_RIGHT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_BLUE_FIELD_LEFT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_BLUE_FIELD_RIGHT);
break;
case T:
newAmbObj = AmbiguousObject(FieldObjects::FO_CORNER_UNKNOWN_T, "T Corner");
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_YELLOW_T_LEFT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_YELLOW_T_RIGHT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_CENTRE_T_LEFT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_CENTRE_T_RIGHT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_BLUE_T_LEFT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_BLUE_T_RIGHT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_PROJECTED_T_YELLOW_LEFT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_PROJECTED_T_YELLOW_RIGHT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_PROJECTED_T_BLUE_LEFT);
newAmbObj.addPossibleObjectID(FieldObjects::FO_CORNER_PROJECTED_T_BLUE_RIGHT);
break;
case X:
// At present a cross is ignored as none exist on the field and this indicates poor detection
return false;
default:
//invalid object - do not push to fieldobjects
errorlog << "CornerPoint::addToExternalFieldObjects - attempt to add invalid CornerPoint object id: " << VFOName(m_id) << endl;
#if VISION_FIELDPOINT_VERBOSITY > 1
debug << "CornerPoint::addToExternalFieldObjects - attempt to add invalid CornerPoint object id: " << VFOName(m_id) << endl;
#endif
return false;
}
//update ambiguous corner and add it to ambiguousFieldObjects
newAmbObj.UpdateVisualObject(Vector3<float>(m_location.relativeRadial.x, m_location.relativeRadial.y, m_location.relativeRadial.z),
Vector3<float>(m_spherical_error.x, m_spherical_error.y, m_spherical_error.z),
Vector2<float>(m_location.angular.x, m_location.angular.y),
Vector2<int>(m_location.screen.x,m_location.screen.y),
Vector2<int>(m_size_on_screen.x,m_size_on_screen.y),
timestamp);
fieldobjects->ambiguousFieldObjects.push_back(newAmbObj);
return true;
}
else {
#if VISION_FIELDPOINT_VERBOSITY > 1
debug << "CornerPoint::addToExternalFieldObjects - invalid" << endl;
#endif
return false;
}
}
void FieldLine::printLabel(std::ostream &out) const
{
out << VFOName(FIELDLINE) << "\tp1: " << m_end_points[0] << "\tp2: " << m_end_points[1];
}
/*!
* @brief Updates the external field objects with this goal.
* @param fieldobjects A pointer to the external field objects.
* @param timestamp The current timestamp to apply to the field objects.
*
* This method uses the id of the goal to determine where to place it, it also
* includes no checks before placing them, and nor should it. For example, if this is
* called on multiple YellowLeftGoals then localisation will only see the last one.
*/
bool Goal::addToExternalFieldObjects(FieldObjects *fieldobjects, float timestamp) const
{
#if VISION_GOAL_VERBOSITY > 1
debug << "Goal::addToExternalFieldObjects - m_id: " << VFOName(m_id) << endl;
debug << " " << *this << endl;
#endif
if(valid) {
#if VISION_GOAL_VERBOSITY > 1
debug << "Goal::addToExternalFieldObjects - valid" << endl;
#endif
AmbiguousObject newAmbObj;
switch(m_id) {
case GOAL_L:
newAmbObj = AmbiguousObject(FieldObjects::FO_YELLOW_GOALPOST_UNKNOWN, "Left Yellow Post");
newAmbObj.addPossibleObjectID(FieldObjects::FO_YELLOW_LEFT_GOALPOST);
newAmbObj.addPossibleObjectID(FieldObjects::FO_BLUE_LEFT_GOALPOST);
break;
case GOAL_R:
newAmbObj = AmbiguousObject(FieldObjects::FO_YELLOW_GOALPOST_UNKNOWN, "Right Yellow Post");
newAmbObj.addPossibleObjectID(FieldObjects::FO_YELLOW_RIGHT_GOALPOST);
newAmbObj.addPossibleObjectID(FieldObjects::FO_BLUE_RIGHT_GOALPOST);
break;
case GOAL_U:
newAmbObj = AmbiguousObject(FieldObjects::FO_YELLOW_GOALPOST_UNKNOWN, "Unknown Yellow Post");
newAmbObj.addPossibleObjectID(FieldObjects::FO_YELLOW_LEFT_GOALPOST);
newAmbObj.addPossibleObjectID(FieldObjects::FO_YELLOW_RIGHT_GOALPOST);
newAmbObj.addPossibleObjectID(FieldObjects::FO_BLUE_LEFT_GOALPOST);
newAmbObj.addPossibleObjectID(FieldObjects::FO_BLUE_RIGHT_GOALPOST);
break;
// case GOAL_Y_L:
// stat_id = FieldObjects::FO_YELLOW_LEFT_GOALPOST;
// stationary = true;
// break;
// case GOAL_Y_R:
// stat_id = FieldObjects::FO_YELLOW_RIGHT_GOALPOST;
// stationary = true;
// break;
// case GOAL_Y_U:
// newAmbObj = AmbiguousObject(FieldObjects::FO_YELLOW_GOALPOST_UNKNOWN, "Unknown Yellow Post");
// newAmbObj.addPossibleObjectID(FieldObjects::FO_YELLOW_LEFT_GOALPOST);
// newAmbObj.addPossibleObjectID(FieldObjects::FO_YELLOW_RIGHT_GOALPOST);
// stationary = false;
// break;
// case GOAL_B_L:
// stat_id = FieldObjects::FO_BLUE_LEFT_GOALPOST;
// stationary = true;
// break;
// case GOAL_B_R:
// stat_id = FieldObjects::FO_BLUE_RIGHT_GOALPOST;
// stationary = true;
// break;
// case GOAL_B_U:
// newAmbObj = AmbiguousObject(FieldObjects::FO_BLUE_GOALPOST_UNKNOWN, "Unknown Blue Post");
// newAmbObj.addPossibleObjectID(FieldObjects::FO_BLUE_LEFT_GOALPOST);
// newAmbObj.addPossibleObjectID(FieldObjects::FO_BLUE_RIGHT_GOALPOST);
// stationary = false;
// break;
default:
//invalid object - do not push to fieldobjects
errorlog << "Goal::addToExternalFieldObjects - attempt to add invalid Goal object id: " << VFOName(m_id) << endl;
#if VISION_GOAL_VERBOSITY > 1
debug << "Goal::addToExternalFieldObjects - attempt to add invalid Goal object id: " << VFOName(m_id) << endl;
#endif
return false;
}
//update ambiguous goal post and add it to ambiguousFieldObjects
newAmbObj.UpdateVisualObject(Vector3<float>(m_location.relativeRadial.x, m_location.relativeRadial.y, m_location.relativeRadial.z),
Vector3<float>(m_spherical_error.x, m_spherical_error.y, m_spherical_error.z),
Vector2<float>(m_location.angular.x, m_location.angular.y),
Vector2<int>(m_location.screen.x,m_location.screen.y),
Vector2<int>(m_size_on_screen.x,m_size_on_screen.y),
timestamp);
fieldobjects->ambiguousFieldObjects.push_back(newAmbObj);
return true;
}
else {
#if VISION_GOAL_VERBOSITY > 1
debug << "Goal::addToExternalFieldObjects - invalid" << endl;
#endif
return false;
}
}
//! @brief Stream output for labelling purposes
void CornerPoint::printLabel(std::ostream& out) const
{
out << VFOName(CORNER) << " " << m_location;
}
