bool ClusterTree::matchChild(Trajectory &traj)
{
double temp = std::numeric_limits<double>::max();
tree<Cluster>::iterator newIt;
//for(tree<Cluster>::sibling_iterator it = m_tree.begin(m_currentCluster); it!=m_tree.end(m_currentCluster); ++it)
for(tree<Cluster>::pre_order_iterator it = m_tree.begin(m_currentCluster); it!=m_tree.end(m_currentCluster); ++it)
{
int ind_lim = traj.findClosest((*it).endX(),(*it).endY());
ROS_INFO("Matching trajectory of length %d but only up to element %d",traj.size(),ind_lim);
ROS_INFO("Distance to Child node %d is %f",(*it).getID(),(*it).clusterDistance(traj,ind_lim));
if((*it).clusterDistance(traj,ind_lim) < temp)
{
temp = (*it).clusterDistance(traj,ind_lim);
newIt = it;
}
}
if(temp < 3.0)
{
ROS_INFO("Matched existing child");
m_currentCluster = newIt;
int ind_lim = traj.findClosest((*newIt).endX(),(*newIt).endY());
for(int i=0; i < ind_lim; i++)
{
int ind = (*m_currentCluster).findClosest(traj.getX(i),traj.getY(i));
ROS_INFO("Bulk Updating %d of %d",ind,((*m_currentCluster).size())-1);
(*m_currentCluster).update(traj.getX(i),traj.getY(i),ind);
if(ind == (*m_currentCluster).size()-1)
{
ROS_INFO("Already at END");
}
}
return true;
}
else
{
ROS_INFO("Need to split, doesn't match existing child nodes");
//Cluster newCluster(traj);
//m_currentCluster = m_tree.append_child(m_currentCluster,newCluster);
//m_lastCluster = m_currentCluster;
return false;
}
}
