沉默杀手HomeArchivesTagsCategories
路径平滑常用代码
|路径规划路径平滑|
Word count: 504|Reading time: 3 min

平均采样，获得anchor的index

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
/**
 * 平均分割区间 [start, end] 为num段，num+1 个点，存入sliced
 * start and end 为sliced的首尾元素
 */
template <typename T>
void uniform_slice(const T start, const T end, uint32_t num,
                   std::vector<T>* sliced)
{
  if (!sliced || num == 0)
        return;
  const T delta = (end - start) / num;
  sliced->resize(num + 1);
  T  s = start;
  for (uint32_t i = 0; i < num; ++i, s += delta)
  {
      sliced->at(i) = s;
  }
  sliced->at(num) = end;
}

补充

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
vector<Point2D> samplePoints(const vector<Point2D>& input_points, int interval)
{
    vector<Point2D> anchor_points;

    int sampling_num = std::max(2, static_cast<int>(input_points.size() / interval + 0.5)  );
    
    vector<double> anchor_id;
    Point2D temp_point;
    uniform_slice(0, input_points.size(), sampling_num - 1, &anchor_id);
    
    for(int i=0; i< anchor_id.size(); i++)
    {
        // cout << anchor_id.at(i) << "   ";
        temp_point = input_points.at(anchor_id.at(i) );
        // cout << "anchor X: " << temp_point.first <<endl;
        // cout << "anchor Y: " << temp_point.second <<endl;
        anchor_points.push_back(temp_point);
    }
    return anchor_points;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
/**
 * Check if two points u and v are the same point on XY dimension.
 * @param u one point that has member function x() and y().
 * @param v one point that has member function x() and y().
 * @return sqrt((u.x-v.x)^2 + (u.y-v.y)^2) < epsilon, i.e., the Euclid distance
 * on XY dimension.
 */
template <typename U, typename V>
bool SamePointXY(const U& u, const V& v)
{
    static constexpr double kMathEpsilonSqr = 1e-8 * 1e-8;
    return (u.x() - v.x()) * (u.x() - v.x()) < kMathEpsilonSqr &&
         (u.y() - v.y()) * (u.y() - v.y()) < kMathEpsilonSqr;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
PathPoint GetWeightedAverageOfTwoPathPoints(const PathPoint& p1,
                                            const PathPoint& p2,
                                            const double w1, const double w2)
{
    PathPoint p;
    p.set_x(p1.x() * w1 + p2.x() * w2);
    p.set_y(p1.y() * w1 + p2.y() * w2);
    p.set_z(p1.z() * w1 + p2.z() * w2);
    p.set_theta(p1.theta() * w1 + p2.theta() * w2);
    p.set_kappa(p1.kappa() * w1 + p2.kappa() * w2);
    p.set_dkappa(p1.dkappa() * w1 + p2.dkappa() * w2);
    p.set_ddkappa(p1.ddkappa() * w1 + p2.ddkappa() * w2);
    p.set_s(p1.s() * w1 + p2.s() * w2);
    return p;
}
1
2
3
4
5
6
7
8
9
10
11
12
// Test whether two float or double numbers are equal.
template <typename T>
typename std::enable_if<!std::numeric_limits<T>::is_integer, bool>::type
IsFloatEqual(T x, T y, int ulp = 2)
{
  // the machine epsilon has to be scaled to the magnitude of the values used
  // and multiplied by the desired precision in ULPs (units in the last place)
  return std::fabs(x - y) <
             std::numeric_limits<T>::epsilon() * std::fabs(x + y) * ulp
         // unless the result is subnormal
         || std::fabs(x - y) < std::numeric_limits<T>::min();
}