Exemple #1
0
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;

    }