本文整理汇总了C++中Trajectory::back方法的典型用法代码示例。如果您正苦于以下问题：C++ Trajectory::back方法的具体用法？C++ Trajectory::back怎么用？C++ Trajectory::back使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Trajectory的用法示例。
在下文中一共展示了Trajectory::back方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: IsStraight
bool IsStraight(Trajectory &trajectory)
{
return boost::geometry::length(trajectory)/boost::geometry::distance(trajectory.front(),trajectory.back()) < 1.05;
}示例2: optimize_trajectory
void RRT::optimize_trajectory(){
Trajectory traj;
double length, new_length;
double v_max = world.leader->get_limits().vMax*0.8;
double c_max = world.leader->get_limits().cMax;
double c_min = world.leader->get_limits().cMin;
State st = states[0],
st2;
for (int i = 0; i < out_trajectory.size(); ++i) {
traj.push_back(Desc(st,out_trajectory[i]));
st2 = world.leader->calculate_state(st, out_trajectory[i]);
st = st2;
}
traj.push_back(Desc(st,Control()));
#ifdef DEBUG_RRT
length = 0;
for (int i = 0; i < traj.size(); ++i) {
length += traj[i].control.t * traj[i].control.v;
}
TimeMeasurement tm;
tm.start();
std::cout << "RRT: original trajectory length: " << length << std::endl;
/*
std::cout << std::endl << "LEADER PLANNED POSITIONS:" << std::endl;
for (int i = 0; i < traj.size(); ++i) {
std::cout << " X = [ " << traj[i].state.X(0) << ", " << traj[i].state.X(1) << ", " << traj[i].state.X(2) << " ]" << std::endl;
}
*/
#endif
int loop = 0;
bool isValid;
bool endPoint;
while(loop < 100){
++loop;
int n_first = rand() % traj.size();
int n_second = rand() % traj.size();
while(n_first == n_second){
n_second = rand() % traj.size();
}
if (n_first > n_second){
const int p = n_first;
n_first = n_second;
n_second = p;
}
length = 0;
for (int i = n_first; i < n_second; ++i) {
length += traj[i].control.t * fabs(traj[i].control.v);
}
endPoint = false;
if (n_second != traj.size()-1){
dubins = new geom::Dubins(geom::Position(geom::Point(traj[n_first].state.X(0),traj[n_first].state.X(1)), traj[n_first].state.phi),
geom::Position(geom::Point(traj[n_second].state.X(0),traj[n_second].state.X(1)), traj[n_second].state.phi),
1/world.leader->cMax());
}else{
endPoint = true;
dubins = new geom::Dubins(geom::Position(geom::Point(traj[n_first].state.X(0),traj[n_first].state.X(1)), traj[n_first].state.phi),
geom::Point(goal[0],goal[1]),
1/world.leader->cMax());
length += dist2D(traj.back().state.X,Vector(goal[0],goal[1],0));
}
new_length = dubins->getLength();
if (new_length < length){
#ifdef DEBUG_RRT_DUBINS
if (endPoint){
std::cout << "DUBINS: original part length : " << length << std::endl;
std::cout << " simplified part length : " << new_length << std::endl;
std::cout << " type of Dubins maneuver: " << dubins->getTypeOfManeuver() << " [RSR, LSL, LSR, RSL, RLR, LRL, LS, RS]" << std::endl;
}
#endif
Trajectory new_traj;
for (int i = 0; i < n_first; ++i) {
new_traj.push_back(traj[i]);
}
double t1, t2, t3, c1, c2, c3;
State state_first = traj[n_first].state;
Control cnt;
switch (dubins->getTypeOfManeuver()) {
case geom::RSR:
t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
t2 = dubins->getLen2()/v_max;
t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
c1 = c_min;
c2 = 0;
c3 = c_min;
break;
case geom::LSL:
t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
t2 = dubins->getLen2()/v_max;
t3 = dubins->getRadius()*fabs(dubins->getLen3())/v_max;
c1 = c_max;
c2 = 0;
c3 = c_max;
break;
case geom::LSR:
t1 = dubins->getRadius()*fabs(dubins->getLen1())/v_max;
//.........这里部分代码省略.........