本文整理汇总了C++中Trajectory::findClosest方法的典型用法代码示例。如果您正苦于以下问题：C++ Trajectory::findClosest方法的具体用法？C++ Trajectory::findClosest怎么用？C++ Trajectory::findClosest使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Trajectory的用法示例。
在下文中一共展示了Trajectory::findClosest方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: matchChild
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;
}
}