/// /// \brief LogicInitiator::processObservations main function which processes new observations /// to initiation candidates. And compares already existing candidates and observation with use of /// cascaded logic. /// \param observations new observations /// \return new tracks to be initated to tracker /// InitiatorCandidates LogicInitiator::processObservations(const Observations observations) { // declare structure for new tracks which should be initiated by tracker InitiatorCandidates newTracks; if(observations.empty()) return newTracks; // declare structure for accepted candidates InitiatorCandidates acceptedCandidates; // Get delta time double timeDiff = observations.front()->createdAt - m_lastObservationTime; m_lastObservationTime = observations.front()->createdAt; // Get observations which could not be matched by the tracker Observations newObs; foreach(Observation::Ptr ob, observations) { if(!ob->matched) { newObs.push_back(ob); } } // Associate new observations with existing candidates for initiation foreach(InitiatorCandidate::Ptr candidate, m_candidates) { // initialize flag if pairing could be found for candidate bool pairingFound(false); // If selected number of scans will be reached tracks are created if(candidate->observations.size() == m_numberScans-1) { predictKalman(candidate->state, timeDiff); candidate->extrapolation = linearExtrapolation(*(candidate->observations.rbegin()+1), candidate->observations.back(),timeDiff); foreach(Observation::Ptr observation,newObs) { if(compareWithExtrapolation(candidate,observation)) { if(compareWithCandidate(candidate, observation)) { if(updateKalman(candidate->state,observation)) { pairingFound = true; candidate->missedObs = 0; observation->matched = true; candidate->observations.push_back(observation); newTracks.push_back(candidate); } } } } }
Observations PositionTracker::makeObservations(DataPtr data) { Observations obsvs; for (int i = 0; i < data->position_size(); i++) { Observation obsv(OBSERVATION_DIMENSIONS); obsv[0] = data->position(i).position().x(); obsv[1] = data->position(i).position().y(); obsv[2] = data->position(i).position().z(); obsvs.push_back(obsv); } return obsvs; }