/*************************************************************** 【函数功能】: 查找中间用于视觉校正的路径点 【输 入】: 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; }