GeoVector
AbortTask::get_vector_home(const AIRCRAFT_STATE &state) const
{
const Waypoint* home_waypoint = waypoints.find_home();
if (home_waypoint) {
return GeoVector(state.Location, home_waypoint->Location);
} else {
GeoVector null_vector(fixed_zero);
return null_vector;
}
}
bool GazeTracker::SixLEDs::associateGlints(const std::vector<cv::Point2d> &glints,
cv::Point2f pc,
const std::vector<cv::Point3d> &glints_3d) {
const size_t sz = glints.size();
if(sz <= 1 || sz >= 7) {
return false;
}
/************************************************************************
* The GroupManager requires an integer coordinate vector. Therefore
* the given double precision vector must be copied to such a container.
************************************************************************/
std::vector<cv::Point> extracted(sz);
{
std::vector<cv::Point>::iterator eit = extracted.begin();
std::vector<cv::Point2d>::const_iterator git = glints.begin();
while(eit != extracted.end()) {
eit->x = (int)(git->x + 0.5);
eit->y = (int)(git->y + 0.5);
++eit;
++git;
}
}
/************************************************************************
* try to identify the glints
************************************************************************/
group::GroupManager grp;
if(!grp.identify(extracted, pc)) {
return false;
}
/************************************************************************
* Initialise the leds vector with null points
************************************************************************/
{
cv::Point3d null_vector(0, 0, 0);
std::vector<LED>::iterator ledit = leds.begin();
while(ledit != leds.end()) {
ledit->setGlintDirection3D(null_vector);
ledit->setGlint2D(cv::Point2d());
ledit->setLabel(-1);
++ledit;
}
}
/***********************************************************************
* Ask the best configuration directly from the pattern finder
**********************************************************************/
{
const group::Configuration &config = grp.getBestConfiguration();
std::vector<group::Element>::const_iterator elit = config.elements.begin();
std::vector<group::Element>::const_iterator end = config.elements.end();
/************************************************************************
* Set the identified glints for the leds, yes it is quite funny that a
* container that supposedly stores LED info, also stores info about the
* glint. Deal with it.
************************************************************************/
int i = 0;
while(elit != end) {
const cv::Point3d &curGlint = glints_3d[i];
/*
* The elements in std::vector<...> elements, are in the same order
* as the std::vector<...> glints_3d
*/
leds[elit->label].setGlintDirection3D(curGlint); // direction vector to the glint
leds[elit->label].setGlint2D(glints[i]); // direction vector to the glint
leds[elit->label].setLabel(elit->label);
++elit;
++i;
}
}
return true;
}
