EdgeWeight Point2PhantomNode::GetMinNodeWeight(NodeID node, m2::PointD const & point) const { static double const kInfinity = numeric_limits<EdgeWeight>::max(); static double const kReadCrossRadiusMercator = 1.0E-4; EdgeWeight minWeight = kInfinity; // Geting nodes by geometry. vector<NodeID> geomNodes; Point2Node p2n(m_routingMapping, geomNodes); m_index.ForEachInRectForMWM(p2n, m2::RectD(point.x - kReadCrossRadiusMercator, point.y - kReadCrossRadiusMercator, point.x + kReadCrossRadiusMercator, point.y + kReadCrossRadiusMercator), scales::GetUpperScale(), m_routingMapping.GetMwmId()); sort(geomNodes.begin(), geomNodes.end()); geomNodes.erase(unique(geomNodes.begin(), geomNodes.end()), geomNodes.end()); // Filtering virtual edges. for (EdgeID const & e : m_routingMapping.m_dataFacade.GetAdjacentEdgeRange(node)) { QueryEdge::EdgeData const data = m_routingMapping.m_dataFacade.GetEdgeData(e, node); if (data.forward && !data.shortcut) minWeight = min(minWeight, data.distance); } for (NodeID const & adjacentNode : geomNodes) { if (adjacentNode == node) continue; for (EdgeID const & e : m_routingMapping.m_dataFacade.GetAdjacentEdgeRange(adjacentNode)) { if (m_routingMapping.m_dataFacade.GetTarget(e) != node) continue; QueryEdge::EdgeData const data = m_routingMapping.m_dataFacade.GetEdgeData(e, adjacentNode); if (!data.shortcut && data.backward) minWeight = min(minWeight, data.distance); } } // If we can't determine edge weight. if (minWeight == kInfinity) return 0; return minWeight; }
/*************************************************************** 【函数功能】: 查找中间用于视觉校正的路径点 【输 入】: 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); // 最后一个校正点 }
int main() { G4double rot2 = 0*deg; Target tar; G4ThreeVector p0n( tar.GetLength()/2.0, tar.GetWidth()/2.0, -tar.GetThickness()/2.0); G4ThreeVector p1n( tar.GetLength()/2.0,-tar.GetWidth()/2.0, -tar.GetThickness()/2.0); G4ThreeVector p2n(-tar.GetLength()/2.0,-tar.GetWidth()/2.0, -tar.GetThickness()/2.0); G4ThreeVector p3n(-tar.GetLength()/2.0, tar.GetWidth()/2.0, -tar.GetThickness()/2.0); G4ThreeVector p0p( tar.GetLength()/2.0, tar.GetWidth()/2.0, tar.GetThickness()/2.0); G4ThreeVector p1p( tar.GetLength()/2.0,-tar.GetWidth()/2.0, tar.GetThickness()/2.0); G4ThreeVector p2p(-tar.GetLength()/2.0,-tar.GetWidth()/2.0, tar.GetThickness()/2.0); G4ThreeVector p3p(-tar.GetLength()/2.0, tar.GetWidth()/2.0, tar.GetThickness()/2.0); G4RotationMatrix rMatrix; G4ThreeVector a1(1,0,0); G4ThreeVector a2(0,1,0); G4ThreeVector a3(-1,0,1); G4RotationMatrix rM1; rM1.set(a1, 90.0*deg); G4RotationMatrix rM2; rM2.set(a2, 45.0*deg); G4RotationMatrix rM3; rM3.set(a3,-45.0*deg + rot2); rMatrix = rM1*rM2*rM3; // rMatrix.print(std::cout); G4RotationMatrix rotMatrix = rMatrix.inverse(); rotMatrix.print(std::cout); std::cout << "phi=" << rotMatrix.getPhi()/deg << std::endl; std::cout << "theta=" << rotMatrix.getTheta()/deg << std::endl; std::cout << "psi=" << rotMatrix.getPsi()/deg << std::endl; G4ThreeVector p0np = rotMatrix*p0n; G4ThreeVector p1np = rotMatrix*p1n; G4ThreeVector p2np = rotMatrix*p2n; G4ThreeVector p3np = rotMatrix*p3n; G4ThreeVector p0pp = rotMatrix*p0p; G4ThreeVector p1pp = rotMatrix*p1p; G4ThreeVector p2pp = rotMatrix*p2p; G4ThreeVector p3pp = rotMatrix*p3p; std::cout << "p0n: " << p0n << " --> " << p0np << std::endl; std::cout << "p1n: " << p1n << " --> " << p1np << std::endl; std::cout << "p2n: " << p2n << " --> " << p2np << std::endl; std::cout << "p3n: " << p3n << " --> " << p3np << std::endl; std::cout << '\n' << std::endl; std::cout << "p0p: " << p0p << " --> " << p0pp << std::endl; std::cout << "p1p: " << p1p << " --> " << p1pp << std::endl; std::cout << "p2p: " << p2p << " --> " << p2pp << std::endl; std::cout << "p3p: " << p3p << " --> " << p3pp << std::endl; std::cout << "\nCenter of the bottom of the target" << std::endl; G4ThreeVector tar_bottom_center = 0.5*(p2np+p3np); std::cout << tar_bottom_center << std::endl; // to compute the offsets // we know that the target is shifted 1.38 cm in the negative x direction // of the rotated coordinate system. This was computed in the Notes/TargetOffsetCalcs.ods // file through the measurement of images of the target in the mount. G4double tar_offset = 1.38*cm; G4ThreeVector xp = rotMatrix.colX(); xp *= -1.0*tar_offset; std::cout << "The target is actually offset from the base" << "\nof the mount slit. The base of the slit then should be" << "\ndisplaced by the following vector" << std::endl; std::cout << xp << " (mm)" << std::endl; std::cout << "\nThe displacement of the target from the is defined in the FragSimDetConstruction class." << "\nThe mount is positioned relative to the position of the target. An absolute" << "\nposition is acquired by adding the recently computed offset to the bottom center" << "\nof the target." << std::endl; std::cout << "\nCenter of mount slit = " << tar_bottom_center+xp << " (mm)" << std::endl; G4double tar_overhang = 0.25*cm; //<-- Calculated in TargetOffsetCalcs.ods // this is the average value of two methods G4double mount_slit_width = 2.54*cm; std::cout << "\nThe target overhangs the edge of the mount as well. To account for this overhang" << "\nthe mount and sh metal plates are shifted in the +y' direction of the rotated coord" << "\nsystem." << std::endl; G4double yp_shift =-1.0*(0.5*(mount_slit_width - tar.GetWidth()) + tar_overhang); G4ThreeVector yp = yp_shift*rotMatrix.colY(); std::cout << "\nOverhang = " << tar_overhang/cm << " cm"; std::cout << "\nShift in yp= " << yp_shift/cm << " cm" << std::endl; std::cout << "\nAlso we know that the target is offset by the thickness of the sheet metal" << "\nin the new z direction. Therefore, there must be a shift downwards" << "\nin the mount along the new z-direction by 0.127 cm." << std::endl; G4double sh_thickness = 0.127*cm; G4ThreeVector zp = rotMatrix.colZ(); zp *= -1.0*sh_thickness; // The following measurements were computed using the 232Th_Inventor_Model_measurements.ods file // in the Ubuntu\ One/AnalysisLog folder. G4double target_x = -0.44*cm; G4double target_y = -0.01*cm; G4double target_z = -1.67*cm; G4ThreeVector targetPos(target_x, target_y, target_z); G4ThreeVector mount_transl = targetPos + tar_bottom_center + xp + yp + zp; std::cout << "\n\nThe target offset was " << targetPos << " (mm)" << std::endl; std::cout << "\n\nSlit center should then be at the following location: " << mount_transl << " (mm)" << std::endl; std::cout << "\n\nThe sheet metal inserts used to clamp the target in " << "\nplace are positioned with the same rotation matrix as " << "\nthe target but with the following translations" << std::endl; G4double sh_length = 2.54*cm; // The way to compute this is by determining the offset from the center of the // target to align the bottom of the insert with the bottom of the target. // These are only aligned in for the 238U data. // Prior to rotation and translation, the centers of both the target and the // sheet metal will be aligned. The difference can then be computed as : G4double sh_shift = 0.5*(tar.GetLength()-sh_length); /////////////////////////////////////////////////////////////////////////////// // ---------------------------------------------------------------------------- // Compute shift for the top plat // // The shift will be in the rotated x direction G4ThreeVector sh_bot_shift_xp = -1.0*(sh_shift + tar_offset) * rotMatrix.colX(); G4double bsheet_shift_offset = 0*cm; G4double bsheet_shift = 0.47*cm + bsheet_shift_offset; sh_bot_shift_xp += bsheet_shift*rotMatrix.colX(); // the next is the actual offset of the sheet metal plate from the bottom of the slit // the calculation is found in TargetOffSetCalcs.ods G4ThreeVector sh_bot_shift_yp = 0.0*cm * rotMatrix.colY();// sh_bot_shift += -0.5*yp; G4ThreeVector sh_bot_shift_zp = -0.5*(tar.GetThickness()+sh_thickness)*rotMatrix.colZ(); G4ThreeVector sh_bot_shift = sh_bot_shift_xp + sh_bot_shift_yp + sh_bot_shift_zp; /////////////////////////////////////////////////////////////////////////////// // ---------------------------------------------------------------------------- // Compute shift for the bottom plate // G4ThreeVector sh_top_shift_xp = -1.0*(sh_shift+tar_offset)*rotMatrix.colX(); // the next is the actual offset of the sheet metal plate from the bottom of the slit // the calculation is found in TargetOffSetCalcs.ods sh_top_shift_xp += 0.3*cm*rotMatrix.colX(); G4ThreeVector sh_top_shift_yp = 0.0*cm * rotMatrix.colY(); // G4ThreeVector sh_top_shift_yp = 0.5*yp; G4ThreeVector sh_top_shift_zp = 0.5*(tar.GetThickness()+sh_thickness)*rotMatrix.colZ(); G4ThreeVector sh_top_shift = sh_top_shift_xp + sh_top_shift_yp + sh_top_shift_zp; ///////////////////////////////////////////////////////////////////////////////// // ------------------------------------------------------------------------------ // Print results // std::cout << "\nRotation (rot2): " << rot2/deg << " degrees" << "\nBot. sh. shift): " << bsheet_shift_offset/cm << " (cm)" << "\n--------------------------------------------" << std::endl; std::cout << "\nmount_transl : " << mount_transl << " (mm)" << std::endl; std::cout << "\nins_bot_transl : " << sh_bot_shift + targetPos << " (mm)" << std::endl; std::cout << "\nins_top_transl : " << sh_top_shift + targetPos << " (mm)" << std::endl; return 0; }