///
/// \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;
}
