apollo::hdmap::PathApproximation类 参考

#include <path.h>

apollo::hdmap::PathApproximation 的协作图:

Public 成员函数
	PathApproximation ()=default
	PathApproximation (const Path &path, const double max_error)
double	max_error () const
const std::vector< int > &	original_ids () const
const std::vector< common::math::LineSegment2d > &	segments () const
bool	GetProjection (const Path &path, const common::math::Vec2d &point, double *accumulate_s, double *lateral, double *distance) const
bool	OverlapWith (const Path &path, const common::math::Box2d &box, double width) const

Protected 成员函数
void	Init (const Path &path)
bool	is_within_max_error (const Path &path, const int s, const int t)
double	compute_max_error (const Path &path, const int s, const int t)
void	InitDilute (const Path &path)
void	InitProjections (const Path &path)

Protected 属性
double	max_error_ = 0
double	max_sqr_error_ = 0
int	num_points_ = 0
std::vector< int >	original_ids_
std::vector< common::math::LineSegment2d >	segments_
std::vector< double >	max_error_per_segment_
std::vector< double >	projections_
double	max_projection_
int	num_projection_samples_ = 0
std::vector< double >	original_projections_
std::vector< double >	max_original_projections_to_left_
std::vector< double >	min_original_projections_to_right_
std::vector< int >	sampled_max_original_projections_to_left_

详细描述

在文件 path.h 第 153 行定义.

构造及析构函数说明

PathApproximation() [1/2]

apollo::hdmap::PathApproximation::PathApproximation ( )

default

PathApproximation() [2/2]

apollo::hdmap::PathApproximation::PathApproximation ( const Path &  path, const double  max_error  )

inline

在文件 path.h 第 156 行定义.

      : max_error_(max_error), max_sqr_error_(max_error * max_error) {
    Init(path);
  }

成员函数说明

compute_max_error()

double apollo::hdmap::PathApproximation::compute_max_error ( const Path &  path, const int  s, const int  t  )

protected

在文件 path.cc 第 1066 行定义.

                                                         {
  if (s + 1 >= t) {
    return 0.0;
  }
  const auto& points = path.path_points();
  const LineSegment2d segment(points[s], points[t]);
  double max_distance_sqr = 0.0;
  for (int i = s + 1; i < t; ++i) {
    max_distance_sqr =
        std::max(max_distance_sqr, segment.DistanceSquareTo(points[i]));
  }
  return sqrt(max_distance_sqr);
}

GetProjection()

bool apollo::hdmap::PathApproximation::GetProjection	(	const Path &	path,
		const common::math::Vec2d &	point,
		double *	accumulate_s,
		double *	lateral,
		double *	distance
	)		const

在文件 path.cc 第 1215 行定义.

                                                                  {
  if (num_points_ == 0) {
    return false;
  }
  if (accumulate_s == nullptr || lateral == nullptr ||
      min_distance == nullptr) {
    return false;
  }
  double min_distance_sqr = std::numeric_limits<double>::infinity();
  int estimate_nearest_segment_idx = -1;
  std::vector<double> distance_sqr_to_segments;
  distance_sqr_to_segments.reserve(segments_.size());
  for (size_t i = 0; i < segments_.size(); ++i) {
    const double distance_sqr = segments_[i].DistanceSquareTo(point);
    distance_sqr_to_segments.push_back(distance_sqr);
    if (distance_sqr < min_distance_sqr) {
      min_distance_sqr = distance_sqr;
      estimate_nearest_segment_idx = static_cast<int>(i);
    }
  }
  if (estimate_nearest_segment_idx < 0) {
    return false;
  }
  const auto& original_segments = path.segments();
  const int num_original_segments = static_cast<int>(original_segments.size());
  const auto& original_accumulated_s = path.accumulated_s();
  double min_distance_sqr_with_error =
      Sqr(sqrt(min_distance_sqr) +
          max_error_per_segment_[estimate_nearest_segment_idx] + max_error_);
  *min_distance = std::numeric_limits<double>::infinity();
  int nearest_segment_idx = -1;
  for (size_t i = 0; i < segments_.size(); ++i) {
    if (distance_sqr_to_segments[i] >= min_distance_sqr_with_error) {
      continue;
    }
    int first_segment_idx = original_ids_[i];
    int last_segment_idx = original_ids_[i + 1] - 1;
    double max_original_projection = std::numeric_limits<double>::infinity();
    if (first_segment_idx < last_segment_idx) {
      const auto& segment = segments_[i];
      const double projection = segment.ProjectOntoUnit(point);
      const double prod_sqr = Sqr(segment.ProductOntoUnit(point));
      if (prod_sqr >= min_distance_sqr_with_error) {
        continue;
      }
      const double scan_distance = sqrt(min_distance_sqr_with_error - prod_sqr);
      const double min_projection = projection - scan_distance;
      max_original_projection = projections_[i] + projection + scan_distance;
      if (min_projection > 0.0) {
        const double limit = projections_[i] + min_projection;
        const int sample_index =
            std::max(0, static_cast<int>(limit / kSampleDistance));
        if (sample_index >= num_projection_samples_) {
          first_segment_idx = last_segment_idx;
        } else {
          first_segment_idx =
              std::max(first_segment_idx,
                       sampled_max_original_projections_to_left_[sample_index]);
          if (first_segment_idx >= last_segment_idx) {
            first_segment_idx = last_segment_idx;
          } else {
            while (first_segment_idx < last_segment_idx &&
                   max_original_projections_to_left_[first_segment_idx + 1] <
                       limit) {
              ++first_segment_idx;
            }
          }
        }
      }
    }
    bool min_distance_updated = false;
    bool is_within_end_point = false;
    for (int idx = first_segment_idx; idx <= last_segment_idx; ++idx) {
      if (min_original_projections_to_right_[idx] > max_original_projection) {
        break;
      }
      const auto& original_segment = original_segments[idx];
      const double x0 = point.x() - original_segment.start().x();
      const double y0 = point.y() - original_segment.start().y();
      const double ux = original_segment.unit_direction().x();
      const double uy = original_segment.unit_direction().y();
      double proj = x0 * ux + y0 * uy;
      double distance = 0.0;
      if (proj < 0.0) {
        if (is_within_end_point) {
          continue;
        }
        is_within_end_point = true;
        distance = hypot(x0, y0);
      } else if (proj <= original_segment.length()) {
        is_within_end_point = true;
        distance = std::abs(x0 * uy - y0 * ux);
      } else {
        is_within_end_point = false;
        if (idx != last_segment_idx) {
          continue;
        }
        distance = original_segment.end().DistanceTo(point);
      }
      if (distance < *min_distance) {
        min_distance_updated = true;
        *min_distance = distance;
        nearest_segment_idx = idx;
      }
    }
    if (min_distance_updated) {
      min_distance_sqr_with_error = Sqr(*min_distance + max_error_);
    }
  }
  if (nearest_segment_idx >= 0) {
    const auto& segment = original_segments[nearest_segment_idx];
    double proj = segment.ProjectOntoUnit(point);
    const double prod = segment.ProductOntoUnit(point);
    if (nearest_segment_idx > 0) {
      proj = std::max(0.0, proj);
    }
    if (nearest_segment_idx + 1 < num_original_segments) {
      proj = std::min(segment.length(), proj);
    }
    *accumulate_s = original_accumulated_s[nearest_segment_idx] + proj;
    if ((nearest_segment_idx == 0 && proj < 0.0) ||
        (nearest_segment_idx + 1 == num_original_segments &&
         proj > segment.length())) {
      *lateral = prod;
    } else {
      *lateral = (prod > 0 ? (*min_distance) : -(*min_distance));
    }
    return true;
  }
  return false;
}

Init()

void apollo::hdmap::PathApproximation::Init ( const Path &  path )

protected

在文件 path.cc 第 1096 行定义.

                                             {
  InitDilute(path);
  InitProjections(path);
}

InitDilute()

void apollo::hdmap::PathApproximation::InitDilute ( const Path &  path )

protected

在文件 path.cc 第 1101 行定义.

                                                   {
  const int num_original_points = path.num_points();
  original_ids_.clear();
  int last_idx = 0;
  while (last_idx < num_original_points - 1) {
    original_ids_.push_back(last_idx);
    int next_idx = last_idx + 1;
    int delta = 2;
    for (; last_idx + delta < num_original_points; delta *= 2) {
      if (!is_within_max_error(path, last_idx, last_idx + delta)) {
        break;
      }
      next_idx = last_idx + delta;
    }
    for (; delta > 0; delta /= 2) {
      if (next_idx + delta < num_original_points &&
          is_within_max_error(path, last_idx, next_idx + delta)) {
        next_idx += delta;
      }
    }
    last_idx = next_idx;
  }
  original_ids_.push_back(last_idx);
  num_points_ = static_cast<int>(original_ids_.size());
  if (num_points_ == 0) {
    return;
  }
 
  segments_.clear();
  segments_.reserve(num_points_ - 1);
  for (int i = 0; i < num_points_ - 1; ++i) {
    segments_.emplace_back(path.path_points()[original_ids_[i]],
                           path.path_points()[original_ids_[i + 1]]);
  }
  max_error_per_segment_.clear();
  max_error_per_segment_.reserve(num_points_ - 1);
  for (int i = 0; i < num_points_ - 1; ++i) {
    max_error_per_segment_.push_back(
        compute_max_error(path, original_ids_[i], original_ids_[i + 1]));
  }
}

InitProjections()

void apollo::hdmap::PathApproximation::InitProjections ( const Path &  path )

protected

在文件 path.cc 第 1143 行定义.

                                                        {
  if (num_points_ == 0) {
    return;
  }
  projections_.clear();
  projections_.reserve(segments_.size() + 1);
  double s = 0.0;
  projections_.push_back(0);
  for (const auto& segment : segments_) {
    s += segment.length();
    projections_.push_back(s);
  }
  const auto& original_points = path.path_points();
  const int num_original_points = static_cast<int>(original_points.size());
  original_projections_.clear();
  original_projections_.reserve(num_original_points);
  for (size_t i = 0; i < projections_.size(); ++i) {
    original_projections_.push_back(projections_[i]);
    if (i + 1 < projections_.size()) {
      const auto& segment = segments_[i];
      for (int idx = original_ids_[i] + 1; idx < original_ids_[i + 1]; ++idx) {
        const double proj = segment.ProjectOntoUnit(original_points[idx]);
        original_projections_.push_back(
            projections_[i] + std::max(0.0, std::min(proj, segment.length())));
      }
    }
  }
 
  // max_p_to_left[i] = max(p[0], p[1], ... p[i]).
  max_original_projections_to_left_.resize(num_original_points);
  double last_projection = -std::numeric_limits<double>::infinity();
  for (int i = 0; i < num_original_points; ++i) {
    last_projection = std::max(last_projection, original_projections_[i]);
    max_original_projections_to_left_[i] = last_projection;
  }
  for (int i = 0; i + 1 < num_original_points; ++i) {
    CHECK_LE(max_original_projections_to_left_[i],
             max_original_projections_to_left_[i + 1] + kMathEpsilon);
  }
 
  // min_p_to_right[i] = min(p[i], p[i + 1], ... p[size - 1]).
  min_original_projections_to_right_.resize(original_projections_.size());
  last_projection = std::numeric_limits<double>::infinity();
  for (int i = num_original_points - 1; i >= 0; --i) {
    last_projection = std::min(last_projection, original_projections_[i]);
    min_original_projections_to_right_[i] = last_projection;
  }
  for (int i = 0; i + 1 < num_original_points; ++i) {
    CHECK_LE(min_original_projections_to_right_[i],
             min_original_projections_to_right_[i + 1] + kMathEpsilon);
  }
 
  // Sample max_p_to_left by sample_distance.
  max_projection_ = projections_.back();
  num_projection_samples_ =
      static_cast<int>(max_projection_ / kSampleDistance) + 1;
  sampled_max_original_projections_to_left_.clear();
  sampled_max_original_projections_to_left_.reserve(num_projection_samples_);
  double proj = 0.0;
  int last_index = 0;
  for (int i = 0; i < num_projection_samples_; ++i) {
    while (last_index + 1 < num_original_points &&
           max_original_projections_to_left_[last_index + 1] < proj) {
      ++last_index;
    }
    sampled_max_original_projections_to_left_.push_back(last_index);
    proj += kSampleDistance;
  }
  CHECK_EQ(sampled_max_original_projections_to_left_.size(),
           static_cast<size_t>(num_projection_samples_));
}

is_within_max_error()

bool apollo::hdmap::PathApproximation::is_within_max_error ( const Path &  path, const int  s, const int  t  )

protected

在文件 path.cc 第 1081 行定义.

                                                         {
  if (s + 1 >= t) {
    return true;
  }
  const auto& points = path.path_points();
  const LineSegment2d segment(points[s], points[t]);
  for (int i = s + 1; i < t; ++i) {
    if (segment.DistanceSquareTo(points[i]) > max_sqr_error_) {
      return false;
    }
  }
  return true;
}

max_error()

double apollo::hdmap::PathApproximation::max_error ( ) const

inline

在文件 path.h 第 160 行定义.

{ return max_error_; }

original_ids()

const std::vector< int > & apollo::hdmap::PathApproximation::original_ids ( ) const

inline

在文件 path.h 第 161 行定义.

{ return original_ids_; }

OverlapWith()

bool apollo::hdmap::PathApproximation::OverlapWith	(	const Path &	path,
		const common::math::Box2d &	box,
		double	width
	)		const

在文件 path.cc 第 1350 行定义.

                                                        {
  if (num_points_ == 0) {
    return false;
  }
  const Vec2d center = box.center();
  const double radius = box.diagonal() / 2.0 + width;
  const double radius_sqr = Sqr(radius);
  const auto& original_segments = path.segments();
  for (size_t i = 0; i < segments_.size(); ++i) {
    const LineSegment2d& segment = segments_[i];
    const double max_error = max_error_per_segment_[i];
    const double radius_sqr_with_error = Sqr(radius + max_error);
    if (segment.DistanceSquareTo(center) > radius_sqr_with_error) {
      continue;
    }
    int first_segment_idx = original_ids_[i];
    int last_segment_idx = original_ids_[i + 1] - 1;
    double max_original_projection = std::numeric_limits<double>::infinity();
    if (first_segment_idx < last_segment_idx) {
      const auto& segment = segments_[i];
      const double projection = segment.ProjectOntoUnit(center);
      const double prod_sqr = Sqr(segment.ProductOntoUnit(center));
      if (prod_sqr >= radius_sqr_with_error) {
        continue;
      }
      const double scan_distance = sqrt(radius_sqr_with_error - prod_sqr);
      const double min_projection = projection - scan_distance;
      max_original_projection = projections_[i] + projection + scan_distance;
      if (min_projection > 0.0) {
        const double limit = projections_[i] + min_projection;
        const int sample_index =
            std::max(0, static_cast<int>(limit / kSampleDistance));
        if (sample_index >= num_projection_samples_) {
          first_segment_idx = last_segment_idx;
        } else {
          first_segment_idx =
              std::max(first_segment_idx,
                       sampled_max_original_projections_to_left_[sample_index]);
          if (first_segment_idx >= last_segment_idx) {
            first_segment_idx = last_segment_idx;
          } else {
            while (first_segment_idx < last_segment_idx &&
                   max_original_projections_to_left_[first_segment_idx + 1] <
                       limit) {
              ++first_segment_idx;
            }
          }
        }
      }
    }
    for (int idx = first_segment_idx; idx <= last_segment_idx; ++idx) {
      if (min_original_projections_to_right_[idx] > max_original_projection) {
        break;
      }
      const auto& original_segment = original_segments[idx];
      if (original_segment.DistanceSquareTo(center) > radius_sqr) {
        continue;
      }
      if (box.DistanceTo(original_segment) <= width) {
        return true;
      }
    }
  }
  return false;
}

segments()

const std::vector< common::math::LineSegment2d > & apollo::hdmap::PathApproximation::segments ( ) const

inline

在文件 path.h 第 162 行定义.

                                                             {
    return segments_;
  }

类成员变量说明

max_error_

double apollo::hdmap::PathApproximation::max_error_ = 0

protected

在文件 path.h 第 182 行定义.

max_error_per_segment_

std::vector<double> apollo::hdmap::PathApproximation::max_error_per_segment_

protected

在文件 path.h 第 188 行定义.

max_original_projections_to_left_

std::vector<double> apollo::hdmap::PathApproximation::max_original_projections_to_left_

protected

在文件 path.h 第 202 行定义.

max_projection_

double apollo::hdmap::PathApproximation::max_projection_

protected

在文件 path.h 第 194 行定义.

max_sqr_error_

double apollo::hdmap::PathApproximation::max_sqr_error_ = 0

protected

在文件 path.h 第 183 行定义.

min_original_projections_to_right_

std::vector<double> apollo::hdmap::PathApproximation::min_original_projections_to_right_

protected

在文件 path.h 第 203 行定义.

num_points_

int apollo::hdmap::PathApproximation::num_points_ = 0

protected

在文件 path.h 第 185 行定义.

num_projection_samples_

int apollo::hdmap::PathApproximation::num_projection_samples_ = 0

protected

在文件 path.h 第 195 行定义.

original_ids_

std::vector<int> apollo::hdmap::PathApproximation::original_ids_

protected

在文件 path.h 第 186 行定义.

original_projections_

std::vector<double> apollo::hdmap::PathApproximation::original_projections_

protected

在文件 path.h 第 199 行定义.

projections_

std::vector<double> apollo::hdmap::PathApproximation::projections_

protected

在文件 path.h 第 193 行定义.

sampled_max_original_projections_to_left_

std::vector<int> apollo::hdmap::PathApproximation::sampled_max_original_projections_to_left_

protected

在文件 path.h 第 204 行定义.

segments_

std::vector<common::math::LineSegment2d> apollo::hdmap::PathApproximation::segments_

protected

在文件 path.h 第 187 行定义.

---

该类的文档由以下文件生成:

• modules/map/pnc_map/path.h
• modules/map/pnc_map/path.cc