/***************************************************************
【函数功能】： 查找中间用于视觉校正的路径点
【输 入】： path : 完整路径队列
calibPoints : 输出找到的校正点
bFound : 输出是否找到
【输 出】： 无
【说 明】： 查找范围限定在机器人坐标系前方1m点为中心，0.8m*0.4m的矩形ROI区域
！！！试验过程中，发现路标前停止位置偏远，因此减少中心点距离为0.85米！！！
***************************************************************/
void CPathAgent::FindCalibPoses(std::deque<math::TPoint2D> &path,
std::deque<math::TPose2D> &calibPoses, bool &bFound)
{
std::deque<SPose2LM> dequePose2LM;
//std::deque<int> idLands;
calibPoses.clear();
bFound = false;
if(path.size() < 6) return; // 避免路径终点被分割成小段了
// 遍历路径点，判断在哪个路径点能看到路标
for (unsigned int i=5; i<(path.size()-5); i++) // 路径【头5点】和【尾5点】不参与。保证不会出现路径起末点小路径的情况
{
TPoint2D p = path.at(i); // 本次的点
TPoint2D n = path.at(i+1); // 后一个点
TPose2D p2n( n - p ); // p点到n点的矢量
if (abs(p2n.x) < 0.000001) // x==0
{
p2n.phi = (p2n.y > 0) ? (M_PI/2) : (-M_PI/2);
}
else
{
float k = p2n.y / p2n.x;
p2n.phi = (p2n.x > 0) ? atan(k) : (atan(k) + M_PI);
}
p2n.phi = wrapToPi(p2n.phi); // 机器人在该路径点的理想朝向角
CPose2D robot(p.x, p.y, p2n.phi);// 此点上机器人的大地位姿
CPose2D center2robot(m_dCalibCenterDistance, 0, 0); // 查找范围的中心点在机器人坐标系内的坐标
CPose2D center = robot + center2robot; // 查找范围的中心点在大地坐标系内的坐标
// 遍历路标，找到距离0.4米内的路标
for (int j=0; j<m_iLandMarkNum; j++)
{
TPose2D Land = m_pLandMarkPose[j]; // 路标的大地坐标
////////////// 换算到视野中心点坐标系内判断 ////////////////
CPose2D LMpose(Land);
CPose2D Land2Center = LMpose - center; // 路标在视野中心点坐标系内的坐标
CPose2D LandCen(0.05, 0.05, 0); // 路标帖的中心在路标自己坐标系内的坐标
CPose2D LMC2ViewCenter = Land2Center + LandCen; // 路标贴中心在视野中心坐标系内的坐标
if (abs(LMC2ViewCenter.x()) < abs(m_dCalibCenterDistance-0.7)) // 前后范围符合
{
if ( ((LMC2ViewCenter.x() >= 0) && (abs(LMC2ViewCenter.y()) < 0.5))
|| ((LMC2ViewCenter.x() < 0) && (abs(LMC2ViewCenter.y()) < 0.4)) )
{
// 先记下这个点，这个路标。下面再一个路标选一个最近的路径点
double ddis = LMC2ViewCenter.distance2DTo(0,0);
SPose2LM tpose2LM(robot.x(), robot.y(), robot.phi(), ddis, j);
dequePose2LM.push_back(tpose2LM);
}
}
///////////////// 换算判断结束 ////////////////////////
//double dis = center.distance2DTo(Land.x, Land.y);
//if (dis <= 0.4) // 0.4m半径的圆范围
//{
// // 这个路标点满足基本的距离条件，需要进一步判断是否在矩形ROI区域
// CPoint2D Land2Center(Land.x - center.x(), Land.y - center.y()); // 路标在center坐标系的坐标
// CPose2D TurnPose(0,0,-center.phi()); // 旋转到机器人朝向的center坐标系
// CPoint2D newLand2Center = TurnPose + Land2Center;
// if (abs(newLand2Center.x()) < 0.2) // 在矩形ROI区域
// {
// // 记下这个点，这个路标。一个路标选一个最近的路径点
// SPose2LM tpose2LM(robot.x(), robot.y(), robot.phi(), j);
// dequePose2LM.push_back(tpose2LM);
// }
//}
}
}
if (dequePose2LM.size() <= 0) // 没找到校正点
{
bFound = false;
return;
}
else
bFound = true;
// 每个路标只取一个最近校正点
TPose2D minPose;
double minDis = 10;
for (unsigned int i=0; i<dequePose2LM.size(); i++)
{
TPose2D tpose(dequePose2LM.at(i).x, dequePose2LM.at(i).y, dequePose2LM.at(i).phi);
int indexLM = dequePose2LM.at(i).index;
//CPose2D robot(tpose);
//CPose2D center2robot(m_dCalibCenterDistance, 0, 0); // 查找范围的中心点在机器人坐标系内的坐标
//CPose2D center = robot + center2robot; // 查找范围的中心点在大地坐标系内的坐标
//double dis = center.distance2DTo(m_pLandMarkPose[indexLM].x, m_pLandMarkPose[indexLM].y);
double dis = dequePose2LM.at(i).dis;
if (dis < minDis)
{
minDis = dis;
minPose = tpose;
}
if((i+1) < dequePose2LM.size())
{
if (dequePose2LM.at(i+1).index != indexLM) // 这个路标挑完了
{
calibPoses.push_back(minPose);
minDis = 10;
}
}
}
calibPoses.push_back(minPose); // 最后一个校正点
}
Matrix<T> tpose(Matrix<T> &M)
{
Matrix<T> Y(M[0].size(0), Vector<T>(M.size()));
tpose(M, Y);
return Y;
}
