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;
}
