Skip to content

A
apollo

Pinoxchio / apollo
与 Fork 源项目一致

从无法访问的项目Fork

通知 2
Star 0
Fork 0
A
apollo

体验新版 GitCode，发现更多精彩内容 >>

提交 f68010fa 编写于 作者: okkoc's avatar okkoc 提交者： Jiangtao Hu
浏览文件
操作

perception:leave hdmap

上级 392f0132
隐藏空白更改
内联 并排
Showing with 360 addition and 358 deletion
modules/perception/map/hdmap/hdmap_input.cc
浏览文件 @ f68010fa
...@@ -24,9 +24,9 @@ ...@@ -24,9 +24,9 @@
// #include "hdmap_definitions.h" // NOLINT // #include "hdmap_definitions.h" // NOLINT
// #include "hdmap_trajectory.h" // NOLINT // #include "hdmap_trajectory.h" // NOLINT
// #include "modules/perception/base/object_pool_types.h" #include "modules/perception/base/object_pool_types.h"
// #include "modules/perception/lib/config_manager/config_manager.h" #include "modules/perception/lib/config_manager/config_manager.h"
// #include "modules/perception/lib/io/file_util.h" #include "modules/perception/lib/io/file_util.h"
namespace apollo { namespace apollo {
namespace perception { namespace perception {
...@@ -34,232 +34,233 @@ namespace map { ...@@ -34,232 +34,233 @@ namespace map {
// using adu::common::math::Polygon2d; // using adu::common::math::Polygon2d;
// using adu::common::math::Vec2d; // using adu::common::math::Vec2d;
// using adu::hdmap::JunctionInfo; using apollo::hdmap::JunctionInfo;
// using adu::hdmap::JunctionInfoConstPtr; using apollo::hdmap::JunctionInfoConstPtr;
// using adu::hdmap::RoiRoadBoundaryPtr; using apollo::hdmap::RoadROIBoundaryPtr;
using apollo::hdmap::JunctionBoundaryPtr;
// using adu::hdmap::BoundaryLine; // using adu::hdmap::BoundaryLine;
// using adu::hdmap::SignalInfoConstPtr; // using adu::hdmap::SignalInfoConstPtr;
// using adu::hdmap::LaneInfoConstPtr; using apollo::hdmap::LaneInfoConstPtr;
// using lib::ConfigManager; using lib::ConfigManager;
// using base::PolygonDType; using base::PolygonDType;
using base::PointD; using base::PointD;
using base::RoadBoundary; using base::RoadBoundary;
// using base::PointDCloud; using base::PointDCloud;
// using base::PointDCloudPtr; using base::PointDCloudPtr;
// HDMapInput // HDMapInput
bool HDMapInput::Init() { bool HDMapInput::Init() {
// lib::MutexLock lock(&mutex_); lib::MutexLock lock(&mutex_);
return InitInternal(); return InitInternal();
} }
bool HDMapInput::InitInternal() { bool HDMapInput::InitInternal() {
// if (inited_) { if (inited_) {
// return true; return true;
// } }
// if (!InitHDMap()) { if (!InitHDMap()) {
// return false; return false;
// } }
// inited_ = true; inited_ = true;
return true; return true;
} }
bool HDMapInput::Reset() { bool HDMapInput::Reset() {
// lib::MutexLock lock(&mutex_); lib::MutexLock lock(&mutex_);
// inited_ = false; inited_ = false;
return InitInternal(); return InitInternal();
} }
bool HDMapInput::InitHDMap() { bool HDMapInput::InitHDMap() {
// hdmap_.reset(new adu::hdmap::HDMap()); hdmap_.reset(new apollo::hdmap::HDMap());
// ConfigManager* config_manager = ConfigManager* config_manager =
// lib::Singleton<ConfigManager>::get_instance(); lib::Singleton<ConfigManager>::get_instance();
// CHECK_NOTNULL(config_manager); CHECK_NOTNULL(config_manager);
// std::string model_name = "HDMapInput"; std::string model_name = "HDMapInput";
// const lib::ModelConfig* model_config = NULL; const lib::ModelConfig* model_config = NULL;
// if (!config_manager->GetModelConfig(model_name, &model_config)) { if (!config_manager->GetModelConfig(model_name, &model_config)) {
// AERROR << "not found model: " << model_name; AERROR << "not found model: " << model_name;
// return false; return false;
// } }
// if (!model_config->get_value("hdmap_sample_step", &hdmap_sample_step_)) { if (!model_config->get_value("hdmap_sample_step", &hdmap_sample_step_)) {
// AERROR << "hdmap_sample_step not found."; AERROR << "hdmap_sample_step not found.";
// hdmap_sample_step_ = 5; hdmap_sample_step_ = 5;
// } }
// if (!model_config->get_value("hdmap_file", &hdmap_file_)) { if (!model_config->get_value("hdmap_file", &hdmap_file_)) {
// AERROR << "hdmap_file not found."; AERROR << "hdmap_file not found.";
// return false; return false;
// } }
// if (!lib::FileUtil::Exists(hdmap_file_)) { if (!lib::FileUtil::Exists(hdmap_file_)) {
// AERROR << "Failed to find hadmap file: " << hdmap_file_; AERROR << "Failed to find hadmap file: " << hdmap_file_;
// return false; return false;
// } }
// if (hdmap_->load_map_from_file(hdmap_file_) != 0) { if (hdmap_->LoadMapFromFile(hdmap_file_) != 0) {
// AERROR << "Failed to load hadmap file: " << hdmap_file_; AERROR << "Failed to load hadmap file: " << hdmap_file_;
// return false; return false;
// } }
// AINFO << "load hdmap file: " << hdmap_file_; AINFO << "load hdmap file: " << hdmap_file_;
return true; return true;
} }
bool HDMapInput::GetRoiHDMapStruct( bool HDMapInput::GetRoiHDMapStruct(
const base::PointD& pointd, const double distance, const base::PointD& pointd, const double distance,
std::shared_ptr<base::HdmapStruct> hdmap_struct_prt) { std::shared_ptr<base::HdmapStruct> hdmap_struct_prt) {
// lib::MutexLock lock(&mutex_); lib::MutexLock lock(&mutex_);
// CHECK_NOTNULL(hdmap_.get()); CHECK_NOTNULL(hdmap_.get());
// // get original road boundary and junction // get original road boundary and junction
// std::vector<RoiRoadBoundaryPtr> road_boundary_vec; std::vector<RoadROIBoundaryPtr> road_boundary_vec;
// std::vector<JunctionInfoConstPtr> junctions_vec; std::vector<JunctionBoundaryPtr> junctions_vec;
// adu::common::hdmap::Point point; apollo::common::PointENU point;
// point.set_x(pointd.x); point.set_x(pointd.x);
// point.set_y(pointd.y); point.set_y(pointd.y);
// point.set_z(pointd.z); point.set_z(pointd.z);
// int status = hdmap_->get_road_boundaries(point, distance, int status = hdmap_->GetRoadBoundaries(point, distance,
// &road_boundary_vec, &road_boundary_vec,
// &junctions_vec); &junctions_vec);
// if (status != 0) { if (status != 0) {
// AERROR << "Failed to get road boundary, point: " AERROR << "Failed to get road boundary, point: "
// << point.DebugString(); << point.DebugString();
// return false; return false;
// } }
// if (hdmap_struct_prt != nullptr) { if (hdmap_struct_prt != nullptr) {
// hdmap_struct_prt->hole_polygons.clear(); hdmap_struct_prt->hole_polygons.clear();
// hdmap_struct_prt->junction_polygons.clear(); hdmap_struct_prt->junction_polygons.clear();
// hdmap_struct_prt->road_boundary.clear(); hdmap_struct_prt->road_boundary.clear();
// hdmap_struct_prt->road_polygons.clear(); hdmap_struct_prt->road_polygons.clear();
// } else { } else {
// return false; return false;
// } }
// // merge boundary and junction // merge boundary and junction
// std::vector<RoadBoundary> road_boundaries; std::vector<RoadBoundary> road_boundaries;
// MergeBoundaryJunction( MergeBoundaryJunction(
// road_boundary_vec, road_boundary_vec,
// junctions_vec, junctions_vec,
// &road_boundaries, &road_boundaries,
// &(hdmap_struct_prt->road_polygons), &(hdmap_struct_prt->road_polygons),
// &(hdmap_struct_prt->junction_polygons)); &(hdmap_struct_prt->junction_polygons));
// // filter road boundary by junction // filter road boundary by junction
// GetRoadBoundaryFilteredByJunctions( GetRoadBoundaryFilteredByJunctions(
// road_boundaries, road_boundaries,
// hdmap_struct_prt->junction_polygons, hdmap_struct_prt->junction_polygons,
// &(hdmap_struct_prt->road_boundary)); &(hdmap_struct_prt->road_boundary));
return true; return true;
} }
// void HDMapInput::MergeBoundaryJunction( void HDMapInput::MergeBoundaryJunction(
// const std::vector<adu::hdmap::RoiRoadBoundaryPtr>& boundary, const std::vector<apollo::hdmap::RoadROIBoundaryPtr>& boundary,
// const std::vector<adu::hdmap::JunctionInfoConstPtr>& junctions, const std::vector<apollo::hdmap::JunctionBoundaryPtr>& junctions,
// std::vector<base::RoadBoundary>* road_boundaries_ptr, std::vector<base::RoadBoundary>* road_boundaries_ptr,
// std::vector<base::PolygonDType>* road_polygons_ptr, std::vector<base::PolygonDType>* road_polygons_ptr,
// std::vector<base::PolygonDType>* junction_polygons_ptr) { std::vector<base::PolygonDType>* junction_polygons_ptr) {
// const int boundary_size = static_cast<int>(boundary.size()); const int boundary_size = static_cast<int>(boundary.size());
// const int junctions_size = static_cast<int>(junctions.size()); const int junctions_size = static_cast<int>(junctions.size());
// const int polygon_size = boundary_size; const int polygon_size = boundary_size;
// road_boundaries_ptr->clear(); road_boundaries_ptr->clear();
// road_polygons_ptr->clear(); road_polygons_ptr->clear();
// junction_polygons_ptr->clear(); junction_polygons_ptr->clear();
// road_polygons_ptr->resize(polygon_size); road_polygons_ptr->resize(polygon_size);
// junction_polygons_ptr->resize(junctions_size); junction_polygons_ptr->resize(junctions_size);
// road_boundaries_ptr->resize(polygon_size); road_boundaries_ptr->resize(polygon_size);
// int polygons_index = 0; int polygons_index = 0;
// // merege boundary // merege boundary
// int step = hdmap_sample_step_; int step = hdmap_sample_step_;
// PointDCloudPtr temp_cloud = base::PointDCloudPool::Instance().Get(); PointDCloudPtr temp_cloud = base::PointDCloudPool::Instance().Get();
// for (int i = 0; i < polygon_size; i++) { for (int i = 0; i < polygon_size; i++) {
// temp_cloud->clear(); temp_cloud->clear();
// const BoundaryLine& left_boundary = boundary[i]->left_boundary; const BoundaryLine& left_boundary = boundary[i]->left_boundary;
// const std::vector<adu::common::hdmap::Point>& left_line_points = const std::vector<apollo::common::PointENU>& left_line_points =
// left_boundary.line_points; left_boundary.line_points;
// ADEBUG << "input left road_boundary size = " << ADEBUG << "input left road_boundary size = " <<
// left_line_points.size(); left_line_points.size();
// step = (left_line_points.size() > 2) ? hdmap_sample_step_ : 1; step = (left_line_points.size() > 2) ? hdmap_sample_step_ : 1;
// for (int idx = 0; idx < left_line_points.size(); idx += step) { for (int idx = 0; idx < left_line_points.size(); idx += step) {
// PointD pointd; PointD pointd;
// pointd.x = left_line_points.at(idx).x(); pointd.x = left_line_points.at(idx).x();
// pointd.y = left_line_points.at(idx).y(); pointd.y = left_line_points.at(idx).y();
// pointd.z = left_line_points.at(idx).z(); pointd.z = left_line_points.at(idx).z();
// temp_cloud->push_back(pointd); temp_cloud->push_back(pointd);
// } }
// DownsamplePoints(temp_cloud, DownsamplePoints(temp_cloud,
// &(road_boundaries_ptr->at(polygons_index).left_boundary)); &(road_boundaries_ptr->at(polygons_index).left_boundary));
// for (int index = 0; for (int index = 0;
// index < road_boundaries_ptr->at(polygons_index).left_boundary.size(); index < road_boundaries_ptr->at(polygons_index).left_boundary.size();
// ++index) { ++index) {
// road_polygons_ptr->at(polygons_index).push_back( road_polygons_ptr->at(polygons_index).push_back(
// road_boundaries_ptr->at(polygons_index).left_boundary[index]); road_boundaries_ptr->at(polygons_index).left_boundary[index]);
// } }
// ADEBUG << "left road_boundary downsample size = " ADEBUG << "left road_boundary downsample size = "
// << road_polygons_ptr->at(polygons_index).size(); << road_polygons_ptr->at(polygons_index).size();
// temp_cloud->clear(); temp_cloud->clear();
// const BoundaryLine& right_boundary = boundary[i]->right_boundary; const BoundaryLine& right_boundary = boundary[i]->right_boundary;
// const std::vector<adu::common::hdmap::Point>& right_line_points const std::vector<apollo::common::PointENU>& right_line_points
// = right_boundary.line_points; = right_boundary.line_points;
// ADEBUG << "input right road_boundary size = " ADEBUG << "input right road_boundary size = "
// << right_line_points.size(); << right_line_points.size();
// step = (right_line_points.size() > 2) ? hdmap_sample_step_ : 1; step = (right_line_points.size() > 2) ? hdmap_sample_step_ : 1;
// for (int idx = 0; idx < right_line_points.size(); idx += step) { for (int idx = 0; idx < right_line_points.size(); idx += step) {
// PointD pointd; PointD pointd;
// pointd.x = right_line_points.at(idx).x(); pointd.x = right_line_points.at(idx).x();
// pointd.y = right_line_points.at(idx).y(); pointd.y = right_line_points.at(idx).y();
// pointd.z = right_line_points.at(idx).z(); pointd.z = right_line_points.at(idx).z();
// temp_cloud->push_back(pointd); temp_cloud->push_back(pointd);
// } }
// DownsamplePoints(temp_cloud, DownsamplePoints(temp_cloud,
// &(road_boundaries_ptr->at(polygons_index).right_boundary)); &(road_boundaries_ptr->at(polygons_index).right_boundary));
// for (int index = 0; for (int index = 0;
// index < index <
// road_boundaries_ptr->at(polygons_index).right_boundary.size(); road_boundaries_ptr->at(polygons_index).right_boundary.size();
// ++index) { ++index) {
// road_polygons_ptr->at(polygons_index).push_back( road_polygons_ptr->at(polygons_index).push_back(
// road_boundaries_ptr->at(polygons_index).right_boundary[ road_boundaries_ptr->at(polygons_index).right_boundary[
// road_boundaries_ptr->at( road_boundaries_ptr->at(
// polygons_index).right_boundary.size() - 1 - index]); polygons_index).right_boundary.size() - 1 - index]);
// } }
// ADEBUG << "right road_boundary downsample size = " ADEBUG << "right road_boundary downsample size = "
// << road_polygons_ptr->at(polygons_index).size(); << road_polygons_ptr->at(polygons_index).size();
// polygons_index++; polygons_index++;
// } }
// // merge junctions // merge junctions
// for (int idx = 0; idx < junctions_size; idx++) { for (int idx = 0; idx < junctions_size; idx++) {
// const Polygon2d& polygon = junctions[idx]->polygon(); const Polygon2d& polygon = junctions[idx]->polygon();
// const std::vector<Vec2d>& points = polygon.points(); const std::vector<Vec2d>& points = polygon.points();
// for (size_t idj = 0; idj < points.size(); ++idj) { for (size_t idj = 0; idj < points.size(); ++idj) {
// PointD pointd; PointD pointd;
// pointd.x = points[idj].x(); pointd.x = points[idj].x();
// pointd.y = points[idj].y(); pointd.y = points[idj].y();
// pointd.z = 0.0; pointd.z = 0.0;
// junction_polygons_ptr->at(idx).push_back(pointd); junction_polygons_ptr->at(idx).push_back(pointd);
// } }
// } }
// } }
bool HDMapInput::GetRoadBoundaryFilteredByJunctions( bool HDMapInput::GetRoadBoundaryFilteredByJunctions(
const std::vector<base::RoadBoundary>& road_boundaries, const std::vector<base::RoadBoundary>& road_boundaries,
const std::vector<base::PointCloud<PointD>>& junctions, const std::vector<base::PointCloud<PointD>>& junctions,
std::vector<base::RoadBoundary>* flt_road_boundaries_ptr) { std::vector<base::RoadBoundary>* flt_road_boundaries_ptr) {
// for (size_t n_rd = 0; n_rd < road_boundaries.size(); ++n_rd) { for (size_t n_rd = 0; n_rd < road_boundaries.size(); ++n_rd) {
// const base::RoadBoundary& temp_road_boundary = road_boundaries[n_rd]; const base::RoadBoundary& temp_road_boundary = road_boundaries[n_rd];
// std::vector<base::PolygonDType> temp_left_boundary_vec; std::vector<base::PolygonDType> temp_left_boundary_vec;
// std::vector<base::PolygonDType> temp_right_boundary_vec; std::vector<base::PolygonDType> temp_right_boundary_vec;
// // filter left boundary points // filter left boundary points
// this->SplitBoundary(temp_road_boundary.left_boundary, this->SplitBoundary(temp_road_boundary.left_boundary,
// junctions, &temp_left_boundary_vec); junctions, &temp_left_boundary_vec);
// // filter right boundary points // filter right boundary points
// this->SplitBoundary(temp_road_boundary.right_boundary, this->SplitBoundary(temp_road_boundary.right_boundary,
// junctions, &temp_right_boundary_vec); junctions, &temp_right_boundary_vec);
// int n_temp_road_boundary = std::max(temp_left_boundary_vec.size(), int n_temp_road_boundary = std::max(temp_left_boundary_vec.size(),
// temp_right_boundary_vec.size()); temp_right_boundary_vec.size());
// for (int i = 0; i < n_temp_road_boundary; ++i) { for (int i = 0; i < n_temp_road_boundary; ++i) {
// base::RoadBoundary temp_road_boundary; base::RoadBoundary temp_road_boundary;
// if (i < temp_left_boundary_vec.size()) { if (i < temp_left_boundary_vec.size()) {
// temp_road_boundary.left_boundary = temp_left_boundary_vec[i]; temp_road_boundary.left_boundary = temp_left_boundary_vec[i];
// } }
// if (i < temp_right_boundary_vec.size()) { if (i < temp_right_boundary_vec.size()) {
// temp_road_boundary.right_boundary = temp_right_boundary_vec[i]; temp_road_boundary.right_boundary = temp_right_boundary_vec[i];
// } }
// flt_road_boundaries_ptr->push_back(temp_road_boundary); flt_road_boundaries_ptr->push_back(temp_road_boundary);
// } }
// } }
return true; return true;
} }
...@@ -267,168 +268,168 @@ void HDMapInput::DownsamplePoints( ...@@ -267,168 +268,168 @@ void HDMapInput::DownsamplePoints(
const std::shared_ptr<base::PointCloud<base::PointD>>& raw_cloud_ptr, const std::shared_ptr<base::PointCloud<base::PointD>>& raw_cloud_ptr,
base::PointCloud<base::PointD>* polygon_ptr, base::PointCloud<base::PointD>* polygon_ptr,
int min_points_num_for_sample) const { int min_points_num_for_sample) const {
// const PointDCloud& raw_cloud = *raw_cloud_ptr; const PointDCloud& raw_cloud = *raw_cloud_ptr;
// unsigned int spt = 0; unsigned int spt = 0;
// double acos_theta = 0.0; double acos_theta = 0.0;
// const double radian_to_degree = 57.29577951308232; const double radian_to_degree = 57.29577951308232;
// const size_t raw_cloud_size = raw_cloud.size(); const size_t raw_cloud_size = raw_cloud.size();
// if (raw_cloud_size <= min_points_num_for_sample) { if (raw_cloud_size <= min_points_num_for_sample) {
// for (size_t i = 0; i < raw_cloud_size; i++) { for (size_t i = 0; i < raw_cloud_size; i++) {
// polygon_ptr->push_back(raw_cloud[i]); polygon_ptr->push_back(raw_cloud[i]);
// } }
// return; return;
// } }
// // the first point // the first point
// polygon_ptr->push_back(raw_cloud[0]); polygon_ptr->push_back(raw_cloud[0]);
// for (size_t idx = 2; idx < raw_cloud_size; ++idx) { for (size_t idx = 2; idx < raw_cloud_size; ++idx) {
// const PointD& point_0 = raw_cloud[spt]; const PointD& point_0 = raw_cloud[spt];
// const PointD& point_1 = raw_cloud[idx - 1]; const PointD& point_1 = raw_cloud[idx - 1];
// const PointD& point_2 = raw_cloud[idx]; const PointD& point_2 = raw_cloud[idx];
// Eigen::Vector2d v1(point_1.x - point_0.x, point_1.y - point_0.y); Eigen::Vector2d v1(point_1.x - point_0.x, point_1.y - point_0.y);
// Eigen::Vector2d v2(point_2.x - point_1.x, point_2.y - point_1.y); Eigen::Vector2d v2(point_2.x - point_1.x, point_2.y - point_1.y);
// double vector_dist = sqrt(v1.cwiseProduct(v1).sum()) double vector_dist = sqrt(v1.cwiseProduct(v1).sum())
// * sqrt(v2.cwiseProduct(v2).sum()); * sqrt(v2.cwiseProduct(v2).sum());
// // judge duplicate points // judge duplicate points
// if (vector_dist < DBL_EPSILON) { if (vector_dist < DBL_EPSILON) {
// continue; continue;
// } }
// double cos_theta = (v1.cwiseProduct(v2)).sum() / vector_dist; double cos_theta = (v1.cwiseProduct(v2)).sum() / vector_dist;
// if (cos_theta > 1.0) { if (cos_theta > 1.0) {
// cos_theta = 1.0; cos_theta = 1.0;
// } else if (cos_theta < -1.0) { } else if (cos_theta < -1.0) {
// cos_theta = -1.0; cos_theta = -1.0;
// } }
// double angle = (acos(cos_theta) * radian_to_degree); double angle = (acos(cos_theta) * radian_to_degree);
// acos_theta += angle; acos_theta += angle;
// if ((acos_theta - 1.0) > DBL_EPSILON) { if ((acos_theta - 1.0) > DBL_EPSILON) {
// polygon_ptr->push_back(point_1); polygon_ptr->push_back(point_1);
// spt = idx - 1; spt = idx - 1;
// acos_theta = 0.0; acos_theta = 0.0;
// } }
// } }
// // the last point // the last point
// polygon_ptr->push_back(raw_cloud[raw_cloud_size - 1]); polygon_ptr->push_back(raw_cloud[raw_cloud_size - 1]);
// AINFO << "Downsample road boundary points from " AINFO << "Downsample road boundary points from "
// << raw_cloud_size << " to " << polygon_ptr->size(); << raw_cloud_size << " to " << polygon_ptr->size();
} }
void HDMapInput::SplitBoundary( void HDMapInput::SplitBoundary(
const base::PointCloud<base::PointD>& boundary_line, const base::PointCloud<base::PointD>& boundary_line,
const std::vector<base::PointCloud<base::PointD>>& junctions, const std::vector<base::PointCloud<base::PointD>>& junctions,
std::vector<base::PointCloud<base::PointD>>* boundary_line_vec_ptr) { std::vector<base::PointCloud<base::PointD>>* boundary_line_vec_ptr) {
// std::vector<bool> boundary_flag(boundary_line.size()); std::vector<bool> boundary_flag(boundary_line.size());
// for (size_t npt = 0; npt < boundary_line.size(); ++npt) { for (size_t npt = 0; npt < boundary_line.size(); ++npt) {
// const PointD& pointd = boundary_line[npt]; const PointD& pointd = boundary_line[npt];
// boundary_flag[npt] = false; boundary_flag[npt] = false;
// for (size_t n_rj = 0; n_rj < junctions.size(); ++n_rj) { for (size_t n_rj = 0; n_rj < junctions.size(); ++n_rj) {
// if (common::IsPointXYInPolygon2DXY(pointd, junctions[n_rj])) { if (common::IsPointXYInPolygon2DXY(pointd, junctions[n_rj])) {
// boundary_flag[npt] = true; boundary_flag[npt] = true;
// break; break;
// } }
// } }
// } }
// std::vector<int> line_index; std::vector<int> line_index;
// base::PolygonDType temp_line; base::PolygonDType temp_line;
// for (size_t i = 1; i < boundary_flag.size(); ++i) { for (size_t i = 1; i < boundary_flag.size(); ++i) {
// if (!boundary_flag[i - 1] || !boundary_flag[i]) { if (!boundary_flag[i - 1] || !boundary_flag[i]) {
// line_index.push_back(i - 1); line_index.push_back(i - 1);
// line_index.push_back(i); line_index.push_back(i);
// } else if (line_index.size() > 1) { } else if (line_index.size() > 1) {
// std::vector<int>::iterator pos; std::vector<int>::iterator pos;
// pos = std::unique(line_index.begin(), line_index.end()); pos = std::unique(line_index.begin(), line_index.end());
// line_index.erase(pos, line_index.end()); line_index.erase(pos, line_index.end());
// for (size_t j = 0; j < line_index.size(); ++j) { for (size_t j = 0; j < line_index.size(); ++j) {
// const PointD& pointd = boundary_line[line_index[j]]; const PointD& pointd = boundary_line[line_index[j]];
// temp_line.push_back(pointd); temp_line.push_back(pointd);
// } }
// boundary_line_vec_ptr->push_back(temp_line); boundary_line_vec_ptr->push_back(temp_line);
// line_index.clear(); line_index.clear();
// temp_line.clear(); temp_line.clear();
// } }
// } }
// // in case the last several unjunctioned points in the "boundary_line" // in case the last several unjunctioned points in the "boundary_line"
// if (line_index.size() > 1) { if (line_index.size() > 1) {
// std::vector<int>::iterator pos; std::vector<int>::iterator pos;
// pos = std::unique(line_index.begin(), line_index.end()); pos = std::unique(line_index.begin(), line_index.end());
// line_index.erase(pos, line_index.end()); line_index.erase(pos, line_index.end());
// for (size_t j = 0; j < line_index.size(); ++j) { for (size_t j = 0; j < line_index.size(); ++j) {
// const PointD& pointd = boundary_line[line_index[j]]; const PointD& pointd = boundary_line[line_index[j]];
// temp_line.push_back(pointd); temp_line.push_back(pointd);
// } }
// boundary_line_vec_ptr->push_back(temp_line); boundary_line_vec_ptr->push_back(temp_line);
// } }
} }
bool HDMapInput::GetNearestLaneDirection(const base::PointD& pointd, bool HDMapInput::GetNearestLaneDirection(const base::PointD& pointd,
Eigen::Vector3d* lane_direction) { Eigen::Vector3d* lane_direction) {
// if (hdmap_ == nullptr) { if (hdmap_ == nullptr) {
// return false; return false;
// } }
// adu::common::hdmap::Point point; apollo::common::PointENU point;
// point.set_x(pointd.x); point.set_x(pointd.x);
// point.set_y(pointd.y); point.set_y(pointd.y);
// point.set_z(pointd.z); point.set_z(pointd.z);
// adu::hdmap::LaneInfoConstPtr nearest_lane; apollo::hdmap::LaneInfoConstPtr nearest_lane;
// double nearest_s = 0.0; double nearest_s = 0.0;
// double nearest_l = 0.0; double nearest_l = 0.0;
// // get nearest lane of query point // get nearest lane of query point
// int status = hdmap_->get_nearest_lane(point, &nearest_lane, int status = hdmap_->GetNearestLane(point, &nearest_lane,
// &nearest_s, &nearest_l); &nearest_s, &nearest_l);
// if (status != 0) { if (status != 0) {
// AINFO << "Failed to get nearest lane for point " << AINFO << "Failed to get nearest lane for point " <<
// point.DebugString(); point.DebugString();
// return false; return false;
// } }
// // get lane heading of nearest s // get lane heading of nearest s
// const adu::hdmap::Trajectory& nearest_lane_trajectory const adu::hdmap::Trajectory& nearest_lane_trajectory
// = nearest_lane->trajectory(); = nearest_lane->trajectory();
// double lane_heading double lane_heading
// = nearest_lane_trajectory.get_smooth_point(nearest_s).heading(); = nearest_lane_trajectory.get_smooth_point(nearest_s).heading();
// *lane_direction = Eigen::Vector3d(cos(lane_heading), sin(lane_heading), 0); *lane_direction = Eigen::Vector3d(cos(lane_heading), sin(lane_heading), 0);
return true; return true;
} }
// bool HDMapInput::GetSignalsFromHDMap(const Eigen::Vector3d& pointd, bool HDMapInput::GetSignalsFromHDMap(const Eigen::Vector3d& pointd,
// double forward_distance, double forward_distance,
// std::vector<adu::common::hdmap::Signal>* signals) { std::vector<apollo::hdmap::Signal>* signals) {
// adu::common::hdmap::Point point; apollo::common::PointENU point;
// point.set_x(pointd(0)); point.set_x(pointd(0));
// point.set_y(pointd(1)); point.set_y(pointd(1));
// point.set_z(pointd(2)); point.set_z(pointd(2));
// std::vector<SignalInfoConstPtr> forward_signals; std::vector<SignalInfoConstPtr> forward_signals;
// // int result = _hdmap->get_forward_signal( // int result = _hdmap->get_forward_signal(
// // point, // point,
// // forward_distance, // forward_distance,
// // &forward_signals); // &forward_signals);
// // int result = _hdmap->get_forward_nearest_signals_on_road( // int result = _hdmap->get_forward_nearest_signals_on_road(
// // point, // point,
// // forward_distance, // forward_distance,
// // &forward_signals); // &forward_signals);
// int result = hdmap_->get_forward_nearest_signals_on_lane( int result = hdmap_->get_forward_nearest_signals_on_lane(
// point, point,
// forward_distance, forward_distance,
// &forward_signals); &forward_signals);
// if (result != 0) { if (result != 0) {
// AERROR << "Failed to call HDMap::get_signal. point: " AERROR << "Failed to call HDMap::get_signal. point: "
// << point.ShortDebugString(); << point.ShortDebugString();
// return false; return false;
// } }
// signals->reserve(forward_signals.size()); signals->reserve(forward_signals.size());
// for (auto& signal_info : forward_signals) { for (auto& signal_info : forward_signals) {
// signals->push_back(signal_info->signal()); signals->push_back(signal_info->signal());
// ADEBUG << "Signal: " << signals->back().DebugString(); ADEBUG << "Signal: " << signals->back().DebugString();
// } }
// ADEBUG << "get_signal success. num_signals: " << signals->size() ADEBUG << "get_signal success. num_signals: " << signals->size()
// << " point: " << point.ShortDebugString(); << " point: " << point.ShortDebugString();
// return true; return true;
// } }
// bool HDMapInput::GetSignals(const Eigen::Vector3d& pointd, bool HDMapInput::GetSignals(const Eigen::Vector3d& pointd,
// double forward_distance, double forward_distance,
// std::vector<adu::common::hdmap::Signal>* signals) { std::vector<apollo::hdmap::Signal>* signals) {
// lib::MutexLock lock(&mutex_); lib::MutexLock lock(&mutex_);
// CHECK_NOTNULL(hdmap_.get()); CHECK_NOTNULL(hdmap_.get());
// return GetSignalsFromHDMap(pointd, forward_distance, signals); return GetSignalsFromHDMap(pointd, forward_distance, signals);
// } }
// add end // add end
} // namespace map } // namespace map
......
modules/perception/map/hdmap/hdmap_input.h
浏览文件 @ f68010fa
...@@ -19,10 +19,11 @@ ...@@ -19,10 +19,11 @@
#include <gflags/gflags.h> #include <gflags/gflags.h>
#include <memory> #include <memory>
// #include <hdmap.h>
#include <string> #include <string>
#include <vector> #include <vector>
#include "modules/perception/base/hdmap_struct.h" #include "modules/perception/base/hdmap_struct.h"
#include "modules/map/hdmap/hdmap.h"
#include "modules/map/hdmap/hdmap_common.h"
// #include "modules/perception/base/point_cloud_types.h" // #include "modules/perception/base/point_cloud_types.h"
#include "modules/perception/lib/singleton/singleton.h" #include "modules/perception/lib/singleton/singleton.h"
// #include "modules/perception/lib/thread/mutex.h" // #include "modules/perception/lib/thread/mutex.h"
...@@ -50,12 +51,12 @@ class HDMapInput { ...@@ -50,12 +51,12 @@ class HDMapInput {
HDMapInput& operator=(const HDMapInput&) = delete; HDMapInput& operator=(const HDMapInput&) = delete;
bool InitHDMap(); bool InitHDMap();
bool InitInternal(); bool InitInternal();
// void MergeBoundaryJunction( void MergeBoundaryJunction(
// const std::vector<adu::hdmap::RoiRoadBoundaryPtr>& boundary, const std::vector<apollo::hdmap::RoadROIBoundaryPtr>& boundary,
// const std::vector<adu::hdmap::JunctionInfoConstPtr>& junctions, const std::vector<apollo::hdmap::JunctionBoundaryPtr>& junctions,
// std::vector<base::RoadBoundary>* road_boundaries_ptr, std::vector<base::RoadBoundary>* road_boundaries_ptr,
// std::vector<base::PolygonDType>* road_polygons_ptr, std::vector<base::PolygonDType>* road_polygons_ptr,
// std::vector<base::PolygonDType>* junction_polygons_ptr); std::vector<base::PolygonDType>* junction_polygons_ptr);
bool GetRoadBoundaryFilteredByJunctions( bool GetRoadBoundaryFilteredByJunctions(
const std::vector<base::RoadBoundary>& road_boundaries, const std::vector<base::RoadBoundary>& road_boundaries,
...@@ -71,14 +72,14 @@ class HDMapInput { ...@@ -71,14 +72,14 @@ class HDMapInput {
const base::PointCloud<base::PointD>& boundary_line, const base::PointCloud<base::PointD>& boundary_line,
const std::vector<base::PointCloud<base::PointD>>& junctions, const std::vector<base::PointCloud<base::PointD>>& junctions,
std::vector<base::PointCloud<base::PointD>>* boundary_line_vec_ptr); std::vector<base::PointCloud<base::PointD>>* boundary_line_vec_ptr);
// bool GetSignalsFromHDMap(const Eigen::Vector3d& pointd, bool GetSignalsFromHDMap(const Eigen::Vector3d& pointd,
// double forward_distance, double forward_distance,
// std::vector<adu::common::hdmap::Signal>* signals); std::vector<apollo::hdmap::Signal>* signals);
bool inited_ = false; bool inited_ = false;
// lib::Mutex mutex_; lib::Mutex mutex_;
// std::unique_ptr<adu::hdmap::HDMap> hdmap_; std::unique_ptr<apollo::hdmap::HDMap> hdmap_;
// int hdmap_sample_step_ = 5; int hdmap_sample_step_ = 5;
// std::string hdmap_file_; std::string hdmap_file_;
friend class lib::Singleton<HDMapInput>; friend class lib::Singleton<HDMapInput>;
}; };
......
Markdown is supported
0% 或 .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑！
想要评论请 注册 或