From f8ede0190de34db984604ce7dce4c83457315a14 Mon Sep 17 00:00:00 2001 From: Jun Zhu Date: Fri, 2 Mar 2018 10:38:46 -0800 Subject: [PATCH] [Perception] add cc_lane_post_processor (#3118) * add type definition and utility for lane post-processing * add connected component analysis code * fix bug in type.h for lane post-processing * modify name of 'connected_components.cc' to 'connected_component.cc' * merge conflict * Perception: fix bugs in connected component analysis * add cc_lane_post_processor * modify object names in lane_post_process/common * fix bugs and coding style issues * delete commented code in 'lane_frame.cc' --- .../obstacle/camera/interface/BUILD | 3 +- .../interface/base_lane_post_processor.h | 100 ++ .../cc_lane_post_processor/BUILD | 38 + .../cc_lane_post_processor.cc | 1015 +++++++++++++++++ .../cc_lane_post_processor.h | 130 +++ .../cc_lane_post_processor/group.h | 474 +------- .../cc_lane_post_processor/lane_frame.cc | 968 ++++++++++++++++ .../cc_lane_post_processor/lane_frame.h | 181 +++ .../camera/lane_post_process/common/BUILD | 12 + .../common/connected_component.cc | 2 - .../lane_post_process/common/projector.h | 482 ++++---- .../camera/lane_post_process/common/type.h | 129 +-- .../camera/lane_post_process/common/util.h | 9 +- 13 files changed, 2747 insertions(+), 796 deletions(-) create mode 100644 modules/perception/obstacle/camera/interface/base_lane_post_processor.h create mode 100644 modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/BUILD create mode 100644 modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor.cc create mode 100644 modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor.h create mode 100644 modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/lane_frame.cc create mode 100644 modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/lane_frame.h diff --git a/modules/perception/obstacle/camera/interface/BUILD b/modules/perception/obstacle/camera/interface/BUILD index 855534b069..eb6dfc21fb 100644 --- a/modules/perception/obstacle/camera/interface/BUILD +++ b/modules/perception/obstacle/camera/interface/BUILD @@ -10,7 +10,8 @@ cc_library( "//modules/perception/lib/base:base", "//modules/perception/obstacle/base:perception_obstacle_base", "//modules/perception/obstacle/camera/common:common", - "@eigen" + "@eigen", + "@opencv2//:core", ] ) diff --git a/modules/perception/obstacle/camera/interface/base_lane_post_processor.h b/modules/perception/obstacle/camera/interface/base_lane_post_processor.h new file mode 100644 index 0000000000..b475094842 --- /dev/null +++ b/modules/perception/obstacle/camera/interface/base_lane_post_processor.h @@ -0,0 +1,100 @@ +/****************************************************************************** + * Copyright 2018 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +// @brief: base class of lane post-processor +// +// SAMPLE CODE: +// +// class DefaultLanePostProcessor : public BaseLanePostProcessor { +// public: +// DefaultLanePostProcessor() : BaseLanePostProcessor() {} +// virtual ~DefaultLanePostProcessor() {} +// +// virtual bool Init() override { +// // Do something. +// return true; +// } +// +// virtual std::string name() const override { +// return "DefaultLanePostProcessor"; +// } +// +// }; +// +// // Register plugin. +// REGISTER_CAMERA_POST_PROCESSOR(DefaultLanePostProcessor); +//////////////////////////////////////////////////////// +// USING CODE: +// +// BaseCameraLanePostProcessor* camera_lane_post_processor = +// BaseCameraLanePostProcessorRegistar::get_instance_by_name("DefaultCameraLanePostProcessor"); +// using camera_detector to do somethings. +// //////////////////////////////////////////////////// + +#ifndef MODULES_PERCEPTION_OBSTACLE_CAMERA_INTERFACE_BASE_LANE_POST_PROC_H_ +#define MODULES_PERCEPTION_OBSTACLE_CAMERA_INTERFACE_BASE_LANE_POST_PROC_H_ + +#include +#include + +#include +#include + +#include "modules/common/macro.h" +#include "modules/perception/lib/base/registerer.h" +#include "modules/perception/obstacle/camera/lane_post_process/common/type.h" + +namespace apollo { +namespace perception { + +struct CameraLanePostProcessOptions { + double timestamp; +}; + +class BaseCameraLanePostProcessor { + public: + BaseCameraLanePostProcessor() = default; + virtual ~BaseCameraLanePostProcessor() = default; + + virtual bool Init() = 0; + + /* + // @brief: lane pixel label map -> lane objects + // @param [in]: lane pixel label map + // @param [in]: options + // @param [out]: lane objects + virtual bool Process(const cv::Mat& lane_map, + const CameraLanePostProcessOptions& options, + apollo::perception::LaneObjectsPtr lane_instances) = 0; + */ + virtual bool Process( + const cv::Mat &lane_map, const CameraLanePostProcessOptions& options, + apollo::perception::LaneObjectsPtr lane_instances) = 0; + + virtual std::string name() const = 0; + + private: + DISALLOW_COPY_AND_ASSIGN(BaseCameraLanePostProcessor); +}; + +REGISTER_REGISTERER(BaseCameraLanePostProcessor); +#define REGISTER_CAMERA_LANE_POST_PROCESSOR(name) \ + REGISTER_CLASS(BaseCameraLanePostProcessor, name) + +} // namespace perception +} // namespace apollo + +#endif // MODULES_PERCEPTION_OBSTACLE_CAMERA_INTERFACE_BASE_LANE_POST_PROC_H_ diff --git a/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/BUILD b/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/BUILD new file mode 100644 index 0000000000..51b663cd34 --- /dev/null +++ b/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/BUILD @@ -0,0 +1,38 @@ +load("//tools:cpplint.bzl", "cpplint") + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "lane_frame", + srcs = ["lane_frame.cc"], + hdrs = [ + "group.h", + "lane_frame.h", + ], + deps = [ + "//modules/common:log", + "//modules/perception/obstacle/camera/lane_post_process/common:perception_obstacle_camera_lane_post_process_common_type", + "//modules/perception/obstacle/camera/lane_post_process/common:perception_obstacle_camera_lane_post_process_common_util", + "//modules/perception/obstacle/camera/lane_post_process/common:perception_obstacle_camera_lane_post_process_common_connected_component_analysis", + "//modules/perception/obstacle/camera/lane_post_process/common:projector", + "@eigen//:eigen", + "@opencv2//:core", + ], +) + +cc_library( + name = "cc_lane_post_processor", + srcs = ["cc_lane_post_processor.cc"], + hdrs = ["cc_lane_post_processor.h"], + deps = [ + "//modules/common:log", + "//modules/perception/lib/config_manager:config_manager", + "//modules/perception/obstacle/camera/common:obstacle_camera_common", + "//modules/perception/obstacle/camera/interface:obstacle_camera_interface", + "//modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor:lane_frame", + "@eigen//:eigen", + "@opencv2//:core", + ], +) + +cpplint() diff --git a/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor.cc b/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor.cc new file mode 100644 index 0000000000..ae3eee1c45 --- /dev/null +++ b/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor.cc @@ -0,0 +1,1015 @@ +/****************************************************************************** + * Copyright 2018 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +// @brief: CC lane post-processor source file + +#include +#include +#include +#include +#include + +#include "modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor.h" + +namespace apollo { +namespace perception { + +using std::pair; +using std::string; +using std::vector; +using std::unordered_map; +using std::shared_ptr; +using std::to_string; + +bool CompOriginLateralDistObjectID(const pair &a, + const pair &b) { + return a.first > b.first; +} + +bool CCLanePostProcessor::Init() { + // 1. get model config + ConfigManager *config_manager = ConfigManager::instance(); + // Singleton::get(); + + const ModelConfig *model_config = NULL; + if (!config_manager->GetModelConfig(this->name(), &model_config)) { + AERROR << "not found model: " << this->name(); + return false; + } + + // 2. get parameters + string space_type; + if (!model_config->GetValue("space_type", &space_type)) { + AERROR << "space type not found."; + return false; + } + if (space_type == "vehicle") { + options_.space_type = SpaceType::VEHICLE; + } else if (space_type == "image") { + AINFO << "using image space to generate lane instances ..."; + options_.space_type = SpaceType::IMAGE; + } else { + AFATAL << "invalid space type" << space_type; + return false; + } + options_.frame.space_type = options_.space_type; + + if (!model_config->GetValue("image_width", &image_width_)) { + AERROR << "image width not found."; + return false; + } + if (!model_config->GetValue("image_height", &image_height_)) { + AERROR << "image height not found."; + return false; + } + + std::vector roi; + if (!model_config->GetValue("roi", &roi)) { + AERROR << "roi not found."; + return false; + } + if (static_cast(roi.size()) != 4) { + AERROR << "roi format error."; + return false; + } else { + roi_.x = static_cast(roi[0]); + roi_.y = static_cast(roi[1]); + roi_.width = static_cast(roi[2]); + roi_.height = static_cast(roi[3]); + options_.frame.image_roi = roi_; + AINFO << "project ROI = [" << roi_.x << ", " << roi_.y << ", " + << roi_.x + roi_.width - 1 << ", " << roi_.y + roi_.height - 1 << "]"; + } + + if (!model_config->GetValue("lane_map_confidence_thresh", + &options_.lane_map_conf_thresh)) { + AERROR << "the confidence threshold of label map not found."; + return false; + } + + if (!model_config->GetValue("cc_split_siz", &options_.cc_split_siz)) { + AERROR << "maximum bounding-box size for splitting CC not found."; + return false; + } + if (!model_config->GetValue("cc_split_len", &options_.cc_split_len)) { + AERROR << "unit length for splitting CC not found."; + return false; + } + + // parameters on generating markers + if (!model_config->GetValue("min_cc_pixel_num", + &options_.frame.min_cc_pixel_num)) { + AERROR << "minimum CC pixel number not found."; + return false; + } + + if (!model_config->GetValue("min_cc_size", &options_.frame.min_cc_size)) { + AERROR << "minimum CC size not found."; + return false; + } + + if (!model_config->GetValue(options_.frame.space_type == SpaceType::IMAGE + ? "min_y_search_offset_image" + : "min_y_search_offset", + &options_.frame.min_y_search_offset)) { + AERROR << "minimum verticle offset used for marker association not found."; + return false; + } + + // parameters on marker association + string assoc_method; + if (!model_config->GetValue("assoc_method", &assoc_method)) { + AERROR << "marker association method not found."; + return false; + } + if (assoc_method == "greedy_group_connect") { + options_.frame.assoc_param.method = AssociationMethod::GREEDY_GROUP_CONNECT; + } else { + AERROR << "invalid marker association method." << assoc_method; + return false; + } + + if (!model_config->GetValue(options_.frame.space_type == SpaceType::IMAGE + ? "assoc_min_distance_image" + : "assoc_min_distance", + &options_.frame.assoc_param.min_distance)) { + AERROR << "minimum distance threshold for marker association not found."; + return false; + } + AINFO << "assoc_min_distance = " << options_.frame.assoc_param.min_distance; + + if (!model_config->GetValue(options_.frame.space_type == SpaceType::IMAGE + ? "assoc_max_distance_image" + : "assoc_max_distance", + &options_.frame.assoc_param.max_distance)) { + AERROR << "maximum distance threshold for marker association not found."; + return false; + } + AINFO << "assoc_max_distance = " << options_.frame.assoc_param.max_distance; + + if (!model_config->GetValue("assoc_distance_weight", + &options_.frame.assoc_param.distance_weight)) { + AERROR << "distance weight for marker association not found."; + return false; + } + AINFO << "assoc_distance_weight = " + << options_.frame.assoc_param.distance_weight; + + if (!model_config->GetValue( + options_.frame.space_type == SpaceType::IMAGE + ? "assoc_max_deviation_angle_image" + : "assoc_max_deviation_angle", + &options_.frame.assoc_param.max_deviation_angle)) { + AERROR << "max deviation angle threshold " + << "for marker association not found."; + return false; + } + AINFO << "assoc_max_deviation_angle = " + << options_.frame.assoc_param.max_deviation_angle; + options_.frame.assoc_param.max_deviation_angle *= (M_PI / 180.0); + + if (!model_config->GetValue( + "assoc_deviation_angle_weight", + &options_.frame.assoc_param.deviation_angle_weight)) { + AERROR << "deviation angle weight " + << "for marker association not found."; + return false; + } + AINFO << "assoc_deviation_angle_weight = " + << options_.frame.assoc_param.deviation_angle_weight; + + if (!model_config->GetValue(options_.frame.space_type == SpaceType::IMAGE + ? "assoc_max_relative_orie_image" + : "assoc_max_relative_orie", + &options_.frame.assoc_param.max_relative_orie)) { + AERROR << "max relative orientation threshold " + << "for marker association not found."; + return false; + } + AINFO << "assoc_max_relative_orie = " + << options_.frame.assoc_param.max_relative_orie; + options_.frame.assoc_param.max_relative_orie *= (M_PI / 180.0); + + if (!model_config->GetValue( + "assoc_relative_orie_weight", + &options_.frame.assoc_param.relative_orie_weight)) { + AERROR << "relative orientation weight " + << "for marker association not found."; + return false; + } + AINFO << "assoc_relative_orie_weight = " + << options_.frame.assoc_param.relative_orie_weight; + + if (!model_config->GetValue( + options_.frame.space_type == SpaceType::IMAGE + ? "assoc_max_departure_distance_image" + : "assoc_max_departure_distance", + &options_.frame.assoc_param.max_departure_distance)) { + AERROR << "max departure distance threshold " + << "for marker association not found."; + return false; + } + AINFO << "assoc_max_departure_distance = " + << options_.frame.assoc_param.max_departure_distance; + + if (!model_config->GetValue( + "assoc_departure_distance_weight", + &options_.frame.assoc_param.departure_distance_weight)) { + AERROR << "departure distance weight " + << "for marker association not found."; + return false; + } + AINFO << "assoc_departure_distance_weight = " + << options_.frame.assoc_param.departure_distance_weight; + + if (!model_config->GetValue( + options_.frame.space_type == SpaceType::IMAGE + ? "assoc_min_orientation_estimation_size_image" + : "assoc_min_orientation_estimation_size", + &options_.frame.assoc_param.min_orientation_estimation_size)) { + AERROR << "minimum size threshold used for orientation estimation" + << " in marker association not found."; + return false; + } + AINFO << "assoc_min_orientation_estimation_size = " + << options_.frame.assoc_param.min_orientation_estimation_size; + + if (options_.frame.assoc_param.method == + AssociationMethod::GREEDY_GROUP_CONNECT) { + if (!model_config->GetValue( + "max_group_prediction_marker_num", + &options_.frame.group_param.max_group_prediction_marker_num)) { + AERROR << "maximum number of markers used for orientation estimation" + << " in greed group connect association not found."; + return false; + } + } else { + AFATAL << "invalid marker association method."; + return false; + } + + if (!model_config->GetValue( + "orientation_estimation_skip_marker_num", + &options_.frame.orientation_estimation_skip_marker_num)) { + AERROR << "skip marker number used for orientation estimation in " + << "marker association"; + return false; + } + + // parameters on finding lane objects + if (!model_config->GetValue("lane_interval_distance", + &options_.frame.lane_interval_distance)) { + AERROR << "The predefined lane interval distance is not found."; + return false; + } + + if (!model_config->GetValue(options_.frame.space_type == SpaceType::IMAGE + ? "min_instance_size_prefiltered_image" + : "min_instance_size_prefiltered", + &options_.frame.min_instance_size_prefiltered)) { + AERROR << "The minimum size of lane instances " + << "to be prefiltered is not found."; + return false; + } + + if (!model_config->GetValue(options_.frame.space_type == SpaceType::IMAGE + ? "max_size_to_fit_straight_line_image" + : "max_size_to_fit_straight_line", + &options_.frame.max_size_to_fit_straight_line)) { + AERROR << "The maximum size used for fitting straight lines " + << "on lane instances is not found."; + return false; + } + + // 3. initialize projector + if (!model_config->GetValue("max_distance_to_see_for_transformer", + &max_distance_to_see_)) { + AERROR << "maximum perception distance for transformer is not found, " + "use default value"; + return false; + } + AINFO << "initial max_distance_to_see: " << max_distance_to_see_ + << " (meters)"; + + if (options_.space_type == SpaceType::VEHICLE) { + projector_.reset(new Projector()); + projector_->Init(roi_, max_distance_to_see_, vis_); + is_x_longitude_ = true; + } else if (options_.space_type == SpaceType::IMAGE) { + is_x_longitude_ = false; + } else { + AFATAL << "invalid space type" << space_type; + return false; + } + + time_stamp_ = 0.0; + frame_id_ = 0; + cc_generator_.reset( + new ConnectedComponentGenerator(image_width_, image_height_, roi_)); + cur_frame_.reset(new LaneFrame); + + is_init_ = true; + return true; +} + +bool CCLanePostProcessor::AddInstanceIntoLaneObject( + const LaneInstance &instance, LaneObject *lane_object) { + if (lane_object == nullptr) { + AERROR << "lane object is a null pointer"; + return false; + } + + if (lane_object->pos.empty()) { + lane_object->pos.reserve(20); + lane_object->orie.reserve(20); + lane_object->image_pos.reserve(20); + lane_object->confidence.reserve(20); + } + + auto graph = cur_frame_->graph(instance.graph_id); + + int i_prev = -1; + for (size_t j = 0; j < graph->size(); ++j) { + int i = graph->at(j).first; + if (cur_frame_->marker(i)->shape_type != MarkerShapeType::POINT) { + if (i_prev < 0 || + cur_frame_->marker(i)->cc_id != cur_frame_->marker(i_prev)->cc_id) { + lane_object->pos.push_back(cur_frame_->marker(i)->start_pos); + lane_object->orie.push_back(cur_frame_->marker(i)->orie); + lane_object->image_pos.push_back( + cur_frame_->marker(i)->image_start_pos); + lane_object->confidence.push_back(cur_frame_->marker(i)->confidence); + lane_object->longitude_start = + std::min(lane_object->pos.back().x(), lane_object->longitude_start); + lane_object->longitude_end = + std::max(lane_object->pos.back().x(), lane_object->longitude_end); + lane_object->point_num++; + } + } + + lane_object->pos.push_back(cur_frame_->marker(i)->pos); + lane_object->orie.push_back(cur_frame_->marker(i)->orie); + lane_object->image_pos.push_back(cur_frame_->marker(i)->image_pos); + lane_object->confidence.push_back(cur_frame_->marker(i)->confidence); + lane_object->longitude_start = + std::min(lane_object->pos.back().x(), lane_object->longitude_start); + lane_object->longitude_end = + std::max(lane_object->pos.back().x(), lane_object->longitude_end); + lane_object->point_num++; + i_prev = i; + } + + if (lane_object->point_num != lane_object->pos.size() || + lane_object->point_num != lane_object->orie.size() || + lane_object->point_num != lane_object->image_pos.size() || + lane_object->point_num != lane_object->confidence.size()) { + AERROR << "the number of points in lane object does not match."; + return false; + } + + if (lane_object->point_num < 2) { + AERROR << "the number of points in lane object should be no less than 2"; + return false; + } + + // fit polynomial model and compute lateral distance for lane object + if (lane_object->point_num < 3 || + lane_object->longitude_end - lane_object->longitude_start < + options_.frame.max_size_to_fit_straight_line) { + // fit a 1st-order polynomial curve (straight line) + lane_object->order = 1; + } else { + // fit a 2nd-order polynomial curve; + lane_object->order = 2; + } + + if (!PolyFit(lane_object->pos, lane_object->order, &(lane_object->model))) { + AERROR << "failed to fit " << lane_object->order + << " order polynomial curve."; + } + lane_object->lateral_distance = lane_object->model(0); + + return true; +} + +bool CCLanePostProcessor::AddInstanceIntoLaneObjectImage( + const LaneInstance &instance, LaneObject *lane_object) { + if (lane_object == nullptr) { + AERROR << "lane object is a null pointer"; + return false; + } + + if (lane_object->pos.empty()) { + lane_object->pos.reserve(20); + lane_object->orie.reserve(20); + lane_object->image_pos.reserve(20); + lane_object->confidence.reserve(20); + } + + auto graph = cur_frame_->graph(instance.graph_id); + + ADEBUG << "show points for lane object: "; + + ScalarType y_offset = static_cast(image_height_ - 1); + + int i_prev = -1; + for (size_t j = 0; j < graph->size(); ++j) { + int i = graph->at(j).first; + if (cur_frame_->marker(i)->shape_type != MarkerShapeType::POINT) { + if (i_prev < 0 || + cur_frame_->marker(i)->cc_id != cur_frame_->marker(i_prev)->cc_id) { + lane_object->pos.push_back(cur_frame_->marker(i)->start_pos); + lane_object->pos.back()(1) = y_offset - lane_object->pos.back()(1); + lane_object->orie.push_back(cur_frame_->marker(i)->orie); + lane_object->orie.back()(1) = -lane_object->orie.back()(1); + lane_object->image_pos.push_back( + cur_frame_->marker(i)->image_start_pos); + lane_object->confidence.push_back(cur_frame_->marker(i)->confidence); + lane_object->longitude_start = + std::min(lane_object->pos.back().y(), lane_object->longitude_start); + lane_object->longitude_end = + std::max(lane_object->pos.back().y(), lane_object->longitude_end); + + ADEBUG << " point " << lane_object->point_num << " = " + << "(" << lane_object->pos.back()(0) << ", " + << lane_object->pos.back()(1) << "), " + << "(" << lane_object->image_pos.back()(0) << ", " + << lane_object->image_pos.back()(1) << ")"; + + lane_object->point_num++; + } + } + + lane_object->pos.push_back(cur_frame_->marker(i)->pos); + lane_object->pos.back()(1) = y_offset - lane_object->pos.back()(1); + lane_object->orie.push_back(cur_frame_->marker(i)->orie); + lane_object->orie.back()(1) = -lane_object->orie.back()(1); + lane_object->image_pos.push_back(cur_frame_->marker(i)->image_pos); + lane_object->confidence.push_back(cur_frame_->marker(i)->confidence); + lane_object->longitude_start = + std::min(lane_object->pos.back().y(), lane_object->longitude_start); + lane_object->longitude_end = + std::max(lane_object->pos.back().y(), lane_object->longitude_end); + + ADEBUG << " point " << lane_object->point_num << " = " + << "(" << lane_object->pos.back()(0) << ", " + << lane_object->pos.back()(1) << "), " + << "(" << lane_object->image_pos.back()(0) << ", " + << lane_object->image_pos.back()(1) << ")"; + + lane_object->point_num++; + i_prev = i; + } + ADEBUG << "longitude_start = " << lane_object->longitude_start; + ADEBUG << "longitude_end = " << lane_object->longitude_end; + + if (lane_object->point_num != lane_object->pos.size() || + lane_object->point_num != lane_object->orie.size() || + lane_object->point_num != lane_object->image_pos.size() || + lane_object->point_num != lane_object->confidence.size()) { + AERROR << "the number of points in lane object does not match."; + return false; + } + + if (lane_object->point_num < 2) { + AERROR << "the number of points in lane object should be no less than 2"; + return false; + } + + // fit polynomial model and compute lateral distance for lane object + ADEBUG << "max_size_to_fit_straight_line = " + << options_.frame.max_size_to_fit_straight_line; + if (lane_object->point_num < 3 || + lane_object->longitude_end - lane_object->longitude_start < + options_.frame.max_size_to_fit_straight_line) { + // fit a 1st-order polynomial curve (straight line) + lane_object->order = 1; + } else { + // fit a 2nd-order polynomial curve; + lane_object->order = 2; + } + + if (!PolyFit(lane_object->pos, lane_object->order, &(lane_object->model), + false)) { + AERROR << "failed to fit " << lane_object->order + << " order polynomial curve"; + } + + lane_object->lateral_distance = lane_object->model(0); + + return true; +} + +bool CCLanePostProcessor::GenerateLaneInstances(const cv::Mat &lane_map) { + if (!is_init_) { + AERROR << "the lane post-processor is not initialized."; + return false; + } + + if (lane_map.empty()) { + AERROR << "input lane map is empty."; + return false; + } + + float time_cur_frame = 0.0f; + Timer timer; + + // 1. get binary lane label mask + timer.tic(); + cv::Mat lane_mask; + if (lane_map.type() == CV_32FC1) { + // confidence heatmap + ADEBUG << "confidence threshold = " << options_.lane_map_conf_thresh; + ADEBUG << "lane map size = " + << "(" << lane_map.cols << ", " << lane_map.rows << ")"; + lane_mask.create(lane_map.rows, lane_map.cols, CV_8UC1); + lane_mask.setTo(cv::Scalar(0)); + for (int h = 0; h < lane_mask.rows; ++h) { + for (int w = 0; w < lane_mask.cols; ++w) { + if (lane_map.at(h, w) >= options_.lane_map_conf_thresh) { + lane_mask.at(h, w) = 1; + } + } + } + } else if (lane_map.type() == CV_8UC1) { + // lane label map + lane_mask = lane_map; + } else { + AERROR << "invalid input lane map type: " << lane_map.type(); + return false; + } + float time_lane_mask = static_cast(timer.toc()); + time_cur_frame += time_lane_mask; + + // 2. find connected components from lane label mask + timer.tic(); + vector cc_list; + cc_generator_->FindConnectedComponents(lane_mask, &cc_list); + + float time_find_cc = static_cast(timer.toc()); + AINFO << "time to find connected components: " << time_find_cc << " ms"; + AINFO << "number of connected components = " << cc_list.size(); + time_cur_frame += time_find_cc; + + // 3. split CC and find inner edges + timer.tic(); + int tot_inner_edge_count = 0; + for (int i = 0; i < static_cast(cc_list.size()); ++i) { + auto it_cc = cc_list[i]; + it_cc->FindBboxPixels(); + it_cc->FindVertices(); + it_cc->FindEdges(); + if (it_cc->DetermineSplit(options_.cc_split_siz) != + ConnectedComponent::BoundingBoxSplitType::NONE) { + it_cc->FindContourForSplit(); + it_cc->SplitContour(options_.cc_split_len); + } + tot_inner_edge_count += static_cast(it_cc->GetInnerEdges()->size()); + } + float time_find_inner_edges = static_cast(timer.toc()); + time_cur_frame += time_find_inner_edges; + AINFO << "time to find inner edges: " << time_find_inner_edges << " ms"; + AINFO << "number of inner edge = " << tot_inner_edge_count; + + /// 4. do lane marker association and determine lane instance labels + timer.tic(); + cur_frame_.reset(new LaneFrame); + + if (options_.frame.space_type == SpaceType::IMAGE) { + cur_frame_->Init(cc_list, options_.frame); + } else if (options_.frame.space_type == SpaceType::VEHICLE) { + cur_frame_->Init(cc_list, projector_, options_.frame); + } else { + AERROR << "unknown space type: " << options_.frame.space_type; + return false; + } + + cur_frame_->Process(cur_lane_instances_); + + float time_lane_frame = static_cast(timer.toc()); + time_cur_frame += time_lane_frame; + AINFO << "time for frame processing: " << time_lane_frame << " ms"; + AINFO << "number of lane instances = " << cur_lane_instances_->size(); + + AINFO << "lane post-processing runtime for current frame: " + << time_cur_frame << " ms"; + + return true; +} + +bool CCLanePostProcessor::Process(const cv::Mat &lane_map, + const CameraLanePostProcessOptions &options, + LaneObjectsPtr lane_objects) { + if (!is_init_) { + AERROR << "lane post-processor is not initialized"; + return false; + } + + if (lane_map.empty()) { + AERROR << "input lane map is empty"; + return false; + } + if (lane_map.cols != image_width_) { + AERROR << "input lane map width does not match: " + << "(" << lane_map.cols << " vs. " << image_width_ << ")"; + return false; + } + if (lane_map.rows != image_height_) { + AERROR << "input lane map height does not match: " + << "(" << lane_map.rows << " vs. " << image_height_ << ")"; + return false; + } + + time_stamp_ = options.timestamp; + + cur_lane_instances_.reset(new vector); + if (!GenerateLaneInstances(lane_map)) { + AERROR << "failed to compute lane instances."; + return false; + } + std::sort(cur_lane_instances_->begin(), cur_lane_instances_->end(), + LaneInstance::CompareSiz); + + /// generate lane objects + Timer timer; + if (options_.space_type == SpaceType::IMAGE) { + /// for image space coordinate + AINFO << "generate lane objects in image space ..."; + ScalarType x_center = static_cast(roi_.x + roi_.width / 2); + + lane_objects.reset(new LaneObjects()); + lane_objects->reserve(2); + bool is_left_lane_found = false; + bool is_right_lane_found = false; + for (auto it = cur_lane_instances_->begin(); + it != cur_lane_instances_->end(); ++it) { + ADEBUG << "for lane instance " << it - cur_lane_instances_->begin(); + + if (is_left_lane_found && is_right_lane_found) { + break; + } + + LaneObject cur_object; + AddInstanceIntoLaneObjectImage(*it, &cur_object); + + if (cur_object.lateral_distance <= x_center) { + // for left lane + if (is_left_lane_found) { + continue; + } + lane_objects->push_back(cur_object); + lane_objects->back().spatial = SpatialLabelType::L_0; + is_left_lane_found = true; + } else { + // for right lane + if (is_right_lane_found) { + continue; + } + lane_objects->push_back(cur_object); + lane_objects->back().spatial = SpatialLabelType::R_0; + is_right_lane_found = true; + } + + AINFO << " lane object " << lane_objects->back().GetSpatialLabel() + << " has " << lane_objects->back().pos.size() << " points: " + << "lateral distance = " << lane_objects->back().lateral_distance; + } + + } else { + /// for vehicle space coordinate + // select lane instances with non-overlap assumption + AINFO << "generate lane objects ..."; + lane_objects.reset(new LaneObjects()); + lane_objects->reserve(2 * MAX_LANE_SPATIAL_LABELS); + vector> origin_lateral_dist_object_id; + origin_lateral_dist_object_id.reserve(2 * MAX_LANE_SPATIAL_LABELS); + int count_lane_objects = 0; + for (auto it = cur_lane_instances_->begin(); + it != cur_lane_instances_->end(); ++it) { + ADEBUG << "for lane instance " << it - cur_lane_instances_->begin(); + + // ignore current instance if it is too small + if (it->siz < options_.frame.min_instance_size_prefiltered) { + ADEBUG << "current instance is too small: " << it->siz; + continue; + } + + LaneObject cur_object; + AddInstanceIntoLaneObject(*it, &cur_object); + + if (lane_objects->empty()) { + // create a new lane object + lane_objects->push_back(cur_object); + origin_lateral_dist_object_id.push_back( + std::make_pair(cur_object.lateral_distance, count_lane_objects++)); + ADEBUG << "generate a new lane object from instance"; + continue; + } + + // ignore current instance if it crosses over any existing groups + bool is_cross_over = false; + vector> lateral_distances( + count_lane_objects); + size_t cross_over_lane_object_id = 0; + for (size_t k = 0; k < lane_objects->size(); ++k) { + // min distance to instance group + lateral_distances[k].first = std::numeric_limits::max(); + // max distance to instance group + lateral_distances[k].second = -std::numeric_limits::max(); + + // determine whether cross over or not + for (size_t i = 0; i < cur_object.pos.size(); ++i) { + ScalarType deta_y = + cur_object.pos[i].y() - PolyEval(cur_object.pos[i].x(), + lane_objects->at(k).order, + lane_objects->at(k).model); + lateral_distances[k].first = + std::min(lateral_distances[k].first, deta_y); + lateral_distances[k].second = + std::max(lateral_distances[k].second, deta_y); + if (lateral_distances[k].first * lateral_distances[k].second < 0) { + is_cross_over = true; + } + if (is_cross_over) { + break; + } + } + + if (is_cross_over) { + cross_over_lane_object_id = k; + break; + } + } + if (is_cross_over) { + ADEBUG << "instance crosses over lane object " + << cross_over_lane_object_id; + continue; + } + + // search the very left lane w.r.t. current instance + int left_lane_id = -1; + ScalarType left_lane_dist = -std::numeric_limits::max(); + for (int k = 0; k < count_lane_objects; ++k) { + if (lateral_distances[k].second <= 0) { + if (lateral_distances[k].second > left_lane_dist) { + left_lane_dist = lateral_distances[k].second; + left_lane_id = k; + } + } + } + if ((left_lane_id >= 0) && + (origin_lateral_dist_object_id.at(left_lane_id).first - + cur_object.lateral_distance < + MIN_BETWEEN_LANE_DISTANCE)) { + ADEBUG << "too close to left lane object (" << left_lane_id << "): " + << origin_lateral_dist_object_id.at(left_lane_id).first - + cur_object.lateral_distance + << "(" << MIN_BETWEEN_LANE_DISTANCE << ")"; + continue; + } + + // search the very right lane w.r.t. current instance + int right_lane_id = -1; + ScalarType right_lane_dist = std::numeric_limits::max(); + for (int k = 0; k < count_lane_objects; ++k) { + if (lateral_distances[k].first > 0) { + if (lateral_distances[k].first < right_lane_dist) { + right_lane_dist = lateral_distances[k].first; + right_lane_id = k; + } + } + } + if ((right_lane_id >= 0) && + (cur_object.lateral_distance - + origin_lateral_dist_object_id.at(right_lane_id).first < + MIN_BETWEEN_LANE_DISTANCE)) { + ADEBUG << "too close to right lane object (" << right_lane_id << "): " + << origin_lateral_dist_object_id.at(right_lane_id).first - + cur_object.lateral_distance + << "(" << MIN_BETWEEN_LANE_DISTANCE << ")"; + continue; + } + + // accept the new lane object + lane_objects->push_back(cur_object); + origin_lateral_dist_object_id.push_back( + std::make_pair(cur_object.lateral_distance, count_lane_objects++)); + ADEBUG << "generate a new lane object from instance."; + } + + // determine spatial label of lane object + std::sort(origin_lateral_dist_object_id.begin(), + origin_lateral_dist_object_id.end(), + CompOriginLateralDistObjectID); + int i_l0 = -1; + for (int k = 0; k < count_lane_objects; ++k) { + if (origin_lateral_dist_object_id[k].first >= 0) { + i_l0 = k; + } else { + break; + } + } + + vector valid_lane_objects; + valid_lane_objects.reserve(lane_objects->size()); + + // for left-side lanes + for (int spatial_index = 0; spatial_index <= i_l0; ++spatial_index) { + if (spatial_index >= MAX_LANE_SPATIAL_LABELS) { + break; + } + int i_l = i_l0 - spatial_index; + int object_id = origin_lateral_dist_object_id.at(i_l).second; + lane_objects->at(object_id).spatial = + static_cast(spatial_index); + valid_lane_objects.push_back(object_id); + + AINFO << " lane object " << lane_objects->at(object_id).GetSpatialLabel() + << " has " << lane_objects->at(object_id).pos.size() << " points: " + << "lateral distance=" + << lane_objects->at(object_id).lateral_distance; + } + + // for right-side lanes + int i_r = i_l0 + 1; + for (int spatial_index = 0; spatial_index < MAX_LANE_SPATIAL_LABELS; + ++spatial_index, ++i_r) { + if (i_r >= count_lane_objects) { + break; + } + int object_id = origin_lateral_dist_object_id.at(i_r).second; + lane_objects->at(object_id).spatial = static_cast( + MAX_LANE_SPATIAL_LABELS + spatial_index); + valid_lane_objects.push_back(object_id); + + AINFO << " lane object " << lane_objects->at(object_id).GetSpatialLabel() + << " has " << lane_objects->at(object_id).pos.size() << " points: " + << "lateral distance=" + << lane_objects->at(object_id).lateral_distance; + } + if (lane_objects->size() != static_cast(count_lane_objects)) { + AERROR << "the number of lane objects does not match."; + return false; + } + + // keep only the lane objects with valid spatial labels + std::sort(valid_lane_objects.begin(), valid_lane_objects.end()); + for (size_t i = 0; i < valid_lane_objects.size(); ++i) { + lane_objects->at(i) = lane_objects->at(valid_lane_objects[i]); + } + lane_objects->resize(valid_lane_objects.size()); + } + + AINFO << "number of lane objects = " << lane_objects->size(); + if (options_.space_type != SpaceType::IMAGE) { + if (!CompensateLaneObjects(lane_objects)) { + AERROR << "fail to compensate lane objects."; + return false; + } + } + EnrichLaneInfo(lane_objects); + float time_generate_lane_objects = static_cast(timer.toc()); + AINFO << "runtime of generating lane objects: " << time_generate_lane_objects; + + return true; +} + +bool CCLanePostProcessor::CompensateLaneObjects(LaneObjectsPtr lane_objects) { + if (lane_objects == NULL) { + AERROR << "lane_objects is a null pointer."; + return false; + } + + bool has_ego_lane_left = false; + int ego_lane_left_idx = -1; + bool has_ego_lane_right = false; + int ego_lane_right_idx = -1; + for (size_t i = 0; i < lane_objects->size(); ++i) { + if (lane_objects->at(i).spatial == SpatialLabelType::L_0) { + has_ego_lane_left = true; + ego_lane_left_idx = static_cast(i); + } else if (lane_objects->at(i).spatial == SpatialLabelType::R_0) { + has_ego_lane_right = true; + ego_lane_right_idx = static_cast(i); + } + } + + if ((has_ego_lane_left && has_ego_lane_right) || + (!has_ego_lane_left && !has_ego_lane_right)) { + return true; + } + + if (!has_ego_lane_left) { + AINFO << "add virtual lane L_0 ..."; + if (ego_lane_right_idx == -1) { + AERROR << "failed to compensate left ego lane due to no right ego lane."; + return false; + } + + LaneObject virtual_ego_lane_left; + lane_objects->at(ego_lane_right_idx).CopyTo(&virtual_ego_lane_left); + + virtual_ego_lane_left.spatial = SpatialLabelType::L_0; + virtual_ego_lane_left.is_compensated = true; + + for (size_t i = 0; i < virtual_ego_lane_left.pos.size(); ++i) { + virtual_ego_lane_left.pos[i](1) += options_.frame.lane_interval_distance; + } + virtual_ego_lane_left.lateral_distance += + options_.frame.lane_interval_distance; + virtual_ego_lane_left.model(0) += options_.frame.lane_interval_distance; + + lane_objects->push_back(virtual_ego_lane_left); + } + + if (!has_ego_lane_right) { + AINFO << "add virtual lane R_0 ..."; + if (ego_lane_left_idx == -1) { + AERROR << "failed to compensate right ego lane due to no left ego lane."; + return false; + } + + LaneObject virtual_ego_lane_right; + lane_objects->at(ego_lane_left_idx).CopyTo(&virtual_ego_lane_right); + + virtual_ego_lane_right.spatial = SpatialLabelType::R_0; + virtual_ego_lane_right.is_compensated = true; + + for (size_t i = 0; i < virtual_ego_lane_right.pos.size(); ++i) { + virtual_ego_lane_right.pos[i](1) -= options_.frame.lane_interval_distance; + } + virtual_ego_lane_right.lateral_distance -= + options_.frame.lane_interval_distance; + virtual_ego_lane_right.model(0) -= options_.frame.lane_interval_distance; + + lane_objects->push_back(virtual_ego_lane_right); + } + + return true; +} + +bool CCLanePostProcessor::EnrichLaneInfo(LaneObjectsPtr lane_objects) { + if (lane_objects == NULL) { + AERROR << "lane_objects is a null pointer."; + return false; + } + + for (size_t i = 0; i < lane_objects->size(); ++i) { + LaneObject &object = (*lane_objects)[i]; + assert(object.pos.size() > 0); + assert(object.image_pos.size() == object.pos.size()); + + // for polynomial curve on vehicle space + object.pos_curve.a = object.model(3); + object.pos_curve.b = object.model(2); + object.pos_curve.c = object.model(1); + object.pos_curve.d = object.model(0); + + // let x_start be always 0 according to L3's PNC requirement + object.pos_curve.x_start = + std::min(object.longitude_start, static_cast(0)); + object.pos_curve.x_end = + std::max(object.longitude_end, static_cast(0)); + + // for polynomial curve on image space + ScalarType img_y_start = static_cast(0); + ScalarType img_y_end = static_cast(INT_MAX); + for (size_t j = 0; j < object.image_pos.size(); ++j) { + ScalarType y = object.image_pos[j](1); + img_y_start = std::max(img_y_start, y); + img_y_end = std::min(img_y_end, y); + } + + // polynomial function: x = a*y^3 + b*y^2 + c*y + d + Eigen::Matrix coeff; + PolyFit(object.image_pos, object.order, &coeff, false); + + object.img_curve.a = coeff(3); + object.img_curve.b = coeff(2); + object.img_curve.c = coeff(1); + object.img_curve.d = coeff(0); + object.img_curve.x_start = img_y_start; + object.img_curve.x_end = img_y_end; + } + + return true; +} + +// Register plugin. +REGISTER_CAMERA_LANE_POST_PROCESSOR(CCLanePostProcessor); + +} // namespace perception +} // namespace apollo diff --git a/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor.h b/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor.h new file mode 100644 index 0000000000..a0f76d0866 --- /dev/null +++ b/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor.h @@ -0,0 +1,130 @@ +/****************************************************************************** + * Copyright 2018 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +// @brief: CC lane post-processor header file + +#ifndef MODULES_PERCEPTION_OBSTACLE_CAMERA_CC_LANE_POST_PROCESSOR_H_ +#define MODULES_PERCEPTION_OBSTACLE_CAMERA_CC_LANE_POST_PROCESSOR_H_ + +#include +#include + +#include +#include +#include +#include + +#include "modules/common/log.h" +#include "modules/perception/lib/config_manager/config_manager.h" +#include "modules/perception/obstacle/camera/common/util.h" +#include "modules/perception/obstacle/camera/interface/base_lane_post_processor.h" +#include "modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/lane_frame.h" + +namespace apollo { +namespace perception { + +struct CCLanePostProcessorOptions { + SpaceType space_type; + ScalarType lane_map_conf_thresh; + ScalarType cc_split_siz; + int cc_split_len; + LaneFrameOptions frame; + + CCLanePostProcessorOptions() + : space_type(SpaceType::VEHICLE), + lane_map_conf_thresh(0.5), + cc_split_siz(100.0), + cc_split_len(50) {} +}; + +class CCLanePostProcessor : public BaseCameraLanePostProcessor { + public: + CCLanePostProcessor() : BaseCameraLanePostProcessor() { + max_distance_to_see_ = 200.0; + vis_ = false; + is_init_ = false; + } + + ~CCLanePostProcessor() {} + + bool Init() override; + + bool Process( + const cv::Mat &lane_map, + const CameraLanePostProcessOptions &options, + LaneObjectsPtr lane_instances) override; + + void set_max_distance_to_see(ScalarType max_distance_to_see) { + max_distance_to_see_ = max_distance_to_see; + } + void set_vis(bool vis) { vis_ = vis; } + + std::string name() const { return "CCLanePostProcessor"; } + + CCLanePostProcessorOptions options() const { return options_; } + + cv::Rect roi() const { return roi_; } + + const std::shared_ptr& cur_frame() { return cur_frame_; } + + std::shared_ptr>& cur_lane_instances() { + return cur_lane_instances_; + } + + protected: + // @brief: add instance into a lane object + bool AddInstanceIntoLaneObject(const LaneInstance &instance, + LaneObject *lane_object); + + // @brief: add instance into a lane object (for "image" mode) + bool AddInstanceIntoLaneObjectImage(const LaneInstance &instance, + LaneObject *lane_object); + + // @brief: generate lane instances from lane map (using lane_frame) + bool GenerateLaneInstances(const cv::Mat &lane_map); + + bool CompensateLaneObjects(LaneObjectsPtr lane_objects); + + bool EnrichLaneInfo(LaneObjectsPtr lane_objects); + + private: + CCLanePostProcessorOptions options_; + + double time_stamp_; + int frame_id_; + std::shared_ptr cc_generator_; + std::shared_ptr cur_frame_; + LaneInstancesPtr cur_lane_instances_; + + ScalarType max_distance_to_see_; + int image_width_; + int image_height_; + cv::Rect roi_; + + bool is_x_longitude_; + + std::shared_ptr> projector_; + + bool is_init_; + bool vis_; + + DISALLOW_COPY_AND_ASSIGN(CCLanePostProcessor); +}; + +} // namespace perception +} // namespace apollo + +#endif // MODULES_PERCEPTION_OBSTACLE_CAMERA_CC_LANE_POST_PROCESSOR_H_ diff --git a/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/group.h b/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/group.h index dabfef4595..7a3c0269e6 100644 --- a/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/group.h +++ b/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/group.h @@ -1,5 +1,5 @@ /****************************************************************************** - * Copyright 2017 The Apollo Authors. All Rights Reserved. + * Copyright 2018 The Apollo Authors. All Rights Reserved. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -17,11 +17,13 @@ #ifndef MODULES_PERCEPTION_OBSTACLE_CAMERA_CC_LANE_POST_PROCESSOR_GROUP_H_ #define MODULES_PERCEPTION_OBSTACLE_CAMERA_CC_LANE_POST_PROCESSOR_GROUP_H_ -#include #include +#include +#include -#include "obstacle/camera/lane_post_process/common/type.h" -#include "obstacle/camera/lane_post_process/common/util.h" +#include "modules/common/log.h" +#include "modules/perception/obstacle/camera/lane_post_process/common/type.h" +#include "modules/perception/obstacle/camera/lane_post_process/common/util.h" namespace apollo { namespace perception { @@ -76,30 +78,22 @@ struct Group { end_angle_list.reserve(MAX_GROUP_PREDICTION_MARKER_NUM); } - static bool comp(const Group& a, const Group& b) { + static bool Comp(const Group& a, const Group& b) { return (a.space_type == SpaceType::IMAGE) ? a.end_pos(1) > b.end_pos(1) : a.end_pos(1) < b.end_pos(1); } - inline int compute_orientation(const std::vector& markers, - const GroupParam& param); - - inline int compute_orientations(const std::vector& markers, - const GroupParam& param); + inline int ComputeOrientation(const std::vector& markers, + const GroupParam& param); - inline ScalarType compute_distance(const Group& tar_group, - const AssociationParam& param) const; + inline ScalarType ComputeDistance(const Group& tar_group, + const AssociationParam& param) const; - inline ScalarType compute_distance( - const Group& tar_group, const AssociationParam& param, - int cur_group_end_marker_descend_id, - int tar_group_start_marker_ascend_id) const; - - inline Bbox bbox(const std::vector& markers) const; + inline Bbox GetBbox(const std::vector& markers) const; }; -inline int Group::compute_orientation(const std::vector& markers, - const GroupParam& group_param) { +inline int Group::ComputeOrientation(const std::vector& markers, + const GroupParam& group_param) { // find valid start marker number int start_marker_count = 0; for (; start_marker_count < static_cast(start_marker_idx.size()); @@ -108,7 +102,7 @@ inline int Group::compute_orientation(const std::vector& markers, break; } } - XLOG(DEBUG) << "start_marker_count = " << start_marker_count; + ADEBUG << "start_marker_count = " << start_marker_count; // compute orientation for start markers switch (shape_type) { @@ -119,38 +113,33 @@ inline int Group::compute_orientation(const std::vector& markers, start_marker_count - 2); } int end_ind = start_marker_count - 1; - XLOG_IF(ERROR, start_ind > end_ind) << "start_ind (" << start_ind - << ") is larger than end_ind (" - << end_ind << ")"; - XLOG(DEBUG) << "start markers: start_ind=" << start_ind - << ", end_ind=" << end_ind; + ADEBUG << "start markers: start_ind=" << start_ind + << ", end_ind=" << end_ind; start_orie = markers.at(start_marker_idx.at(end_ind)).pos - markers.at(start_marker_idx.at(start_ind)).pos; break; } + case MarkerShapeType::LINE_SEGMENT: { int start_ind = std::min(group_param.orientation_estimation_skip_marker_num, start_marker_count - 1); int end_ind = start_marker_count - 1; - XLOG_IF(ERROR, start_ind > end_ind) << "start_ind (" << start_ind - << ") is larger than end_ind (" - << end_ind << ")"; - XLOG(DEBUG) << "start markers: start_ind=" << start_ind - << ", end_ind=" << end_ind; + ADEBUG << "start markers: start_ind=" << start_ind + << ", end_ind=" << end_ind; start_orie = markers.at(start_marker_idx.at(end_ind)).pos - markers.at(start_marker_idx.at(start_ind)).start_pos; break; } - case MarkerShapeType::POLYNOMIAL: { - break; + + default: { + AFATAL << "unknown marker shape type."; } - default: { XLOG(FATAL) << "unknown marker shape type."; } } start_angle = std::atan2(start_orie(1), start_orie(0)); - rect_angle(start_angle); + RectAngle(&start_angle); // find valid end marker number int end_marker_count = 0; @@ -160,7 +149,7 @@ inline int Group::compute_orientation(const std::vector& markers, break; } } - XLOG(DEBUG) << "end_marker_count = " << end_marker_count; + ADEBUG << "end_marker_count = " << end_marker_count; // compute orientation for end markers switch (shape_type) { @@ -171,196 +160,46 @@ inline int Group::compute_orientation(const std::vector& markers, end_ind = std::min(group_param.orientation_estimation_skip_marker_num, end_marker_count - 2); } - XLOG_IF(ERROR, start_ind < end_ind) << "start_ind (" << start_ind - << ") is smaller than end_ind (" - << end_ind << ")"; - XLOG(DEBUG) << "end markers: start_ind=" << start_ind - << ", end_ind=" << end_ind; + ADEBUG << "end markers: start_ind=" << start_ind + << ", end_ind=" << end_ind; end_orie = markers.at(end_marker_idx.at(end_ind)).pos - markers.at(end_marker_idx.at(start_ind)).pos; break; } + case MarkerShapeType::LINE_SEGMENT: { int start_ind = end_marker_count - 1; int end_ind = std::min(group_param.orientation_estimation_skip_marker_num, end_marker_count - 1); - XLOG_IF(ERROR, start_ind < end_ind) << "start_ind (" << start_ind - << ") is smaller than end_ind (" - << end_ind << ")"; - XLOG(DEBUG) << "end markers: start_ind=" << start_ind - << ", end_ind=" << end_ind; + ADEBUG << "end markers: start_ind=" << start_ind + << ", end_ind=" << end_ind; end_orie = markers.at(end_marker_idx.at(end_ind)).pos - markers.at(end_marker_idx.at(start_ind)).start_pos; break; } - case MarkerShapeType::POLYNOMIAL: { - break; - } - default: { XLOG(FATAL) << "unknown marker shape type."; } - } - end_angle = std::atan2(end_orie(1), end_orie(0)); - rect_angle(end_angle); - - return end_marker_count; -} - -inline int Group::compute_orientations(const std::vector& markers, - const GroupParam& group_param) { - // find valid start marker number - int start_marker_count = 0; - for (; start_marker_count < static_cast(start_marker_idx.size()); - ++start_marker_count) { - if (start_marker_idx[start_marker_count] == -1) { - break; - } - } - XLOG(DEBUG) << "start_marker_count = " << start_marker_count; - - // compute orientation for start markers - switch (shape_type) { - case MarkerShapeType::POINT: { - int start_ind = 0; - if (start_marker_count > 1) { - start_ind = std::min(group_param.orientation_estimation_skip_marker_num, - start_marker_count - 2); - } - int end_ind = start_marker_count - 1; - XLOG_IF(ERROR, start_ind > end_ind) << "start_ind (" << start_ind - << ") is larger than end_ind (" - << end_ind << ")"; - XLOG(DEBUG) << "start markers: start_ind=" << start_ind - << ", end_ind=" << end_ind; - this->start_orie = markers.at(start_marker_idx.at(end_ind)).pos - - markers.at(start_marker_idx.at(start_ind)).pos; - for (start_ind = 0; start_ind <= end_ind; ++start_ind) { - this->start_pos_list.push_back( - markers.at(start_marker_idx.at(start_ind)).pos); - this->start_orie_list.push_back( - markers.at(start_marker_idx.at(end_ind)).pos - - markers.at(start_marker_idx.at(start_ind)).pos); - } - break; - } - case MarkerShapeType::LINE_SEGMENT: { - int start_ind = - std::min(group_param.orientation_estimation_skip_marker_num, - start_marker_count - 1); - int end_ind = start_marker_count - 1; - XLOG_IF(ERROR, start_ind > end_ind) << "start_ind (" << start_ind - << ") is larger than end_ind (" - << end_ind << ")"; - XLOG(DEBUG) << "start markers: start_ind=" << start_ind - << ", end_ind=" << end_ind; - this->start_orie = markers.at(start_marker_idx.at(end_ind)).pos - - markers.at(start_marker_idx.at(start_ind)).start_pos; - for (start_ind = 0; start_ind <= end_ind; ++start_ind) { - this->start_pos_list.push_back( - markers.at(start_marker_idx.at(start_ind)).start_pos); - this->start_orie_list.push_back( - markers.at(start_marker_idx.at(end_ind)).pos - - markers.at(start_marker_idx.at(start_ind)).start_pos); - } - break; - } - case MarkerShapeType::POLYNOMIAL: { - break; - } - default: { XLOG(FATAL) << "unknown marker shape type."; } - } - this->start_angle = std::atan2(this->start_orie(1), this->start_orie(0)); - rect_angle(this->start_angle); - for (auto it_start_orie = this->start_orie_list.begin(); - it_start_orie != this->start_orie_list.end(); ++it_start_orie) { - this->start_angle_list.push_back( - std::atan2((*it_start_orie)(1), (*it_start_orie)(0))); - rect_angle(this->start_angle_list.back()); - } - // find valid end marker number - int end_marker_count = 0; - for (; end_marker_count < static_cast(end_marker_idx.size()); - ++end_marker_count) { - if (end_marker_idx[end_marker_count] == -1) { - break; + default: { + AFATAL << "unknown marker shape type."; } } - XLOG(DEBUG) << "end_marker_count = " << end_marker_count; - CHECK_EQ(start_marker_count, end_marker_count); - - // compute orientation for end markers - switch (shape_type) { - case MarkerShapeType::POINT: { - int start_ind = end_marker_count - 1; - int end_ind = 0; - if (end_marker_count > 1) { - end_ind = std::min(group_param.orientation_estimation_skip_marker_num, - end_marker_count - 2); - } - XLOG_IF(ERROR, start_ind < end_ind) << "start_ind (" << start_ind - << ") is smaller than end_ind (" - << end_ind << ")"; - XLOG(DEBUG) << "end markers: start_ind=" << start_ind - << ", end_ind=" << end_ind; - this->end_orie = markers.at(end_marker_idx.at(end_ind)).pos - - markers.at(end_marker_idx.at(start_ind)).pos; - for (end_ind = 0; end_ind <= start_ind; ++end_ind) { - this->end_pos_list.push_back( - markers.at(end_marker_idx.at(end_ind)).pos); - this->end_orie_list.push_back( - markers.at(end_marker_idx.at(end_ind)).pos - - markers.at(end_marker_idx.at(start_ind)).pos); - } - break; - } - case MarkerShapeType::LINE_SEGMENT: { - int start_ind = end_marker_count - 1; - int end_ind = std::min(group_param.orientation_estimation_skip_marker_num, - end_marker_count - 1); - XLOG_IF(ERROR, start_ind < end_ind) << "start_ind (" << start_ind - << ") is smaller than end_ind (" - << end_ind << ")"; - XLOG(DEBUG) << "end markers: start_ind=" << start_ind - << ", end_ind=" << end_ind; - this->end_orie = markers.at(end_marker_idx.at(end_ind)).pos - - markers.at(end_marker_idx.at(start_ind)).start_pos; - for (end_ind = 0; end_ind <= start_ind; ++end_ind) { - this->end_pos_list.push_back( - markers.at(end_marker_idx.at(end_ind)).pos); - this->end_orie_list.push_back( - markers.at(end_marker_idx.at(end_ind)).pos - - markers.at(end_marker_idx.at(start_ind)).start_pos); - } - break; - } - case MarkerShapeType::POLYNOMIAL: { - break; - } - default: { XLOG(FATAL) << "unknown marker shape type."; } - } - this->end_angle = std::atan2(this->end_orie(1), this->end_orie(0)); - rect_angle(this->end_angle); - for (auto it_end_orie = this->end_orie_list.begin(); - it_end_orie != this->end_orie_list.end(); ++it_end_orie) { - this->end_angle_list.push_back( - std::atan2((*it_end_orie)(1), (*it_end_orie)(0))); - rect_angle(this->end_angle_list.back()); - } + end_angle = std::atan2(end_orie(1), end_orie(0)); + RectAngle(&end_angle); return end_marker_count; } -inline ScalarType Group::compute_distance(const Group& tar_group, - const AssociationParam& param) const { - XLOG(DEBUG) << "source point = (" << this->end_pos(0) << ", " - << this->end_pos(1) << ")"; - XLOG(DEBUG) << "target point = (" << tar_group.start_pos(0) << ", " - << tar_group.start_pos(1) << ")"; +inline ScalarType Group::ComputeDistance(const Group& tar_group, + const AssociationParam& param) const { + ADEBUG << "source point = (" << this->end_pos(0) << ", " + << this->end_pos(1) << ")"; + ADEBUG << "target point = (" << tar_group.start_pos(0) << ", " + << tar_group.start_pos(1) << ")"; Vector2D displacement = tar_group.start_pos - this->end_pos; ScalarType norm_v = this->end_orie.norm(); if (norm_v < kEpsilon) { - XLOG(DEBUG) + ADEBUG << "norm of orientation vector for reference group is too small: " << norm_v; } @@ -370,9 +209,9 @@ inline ScalarType Group::compute_distance(const Group& tar_group, if (norm_v >= kEpsilon) { projection_dist /= norm_v; } - XLOG(DEBUG) << "(1) projection_dist = " << std::to_string(projection_dist) - << " [" << std::to_string(param.min_distance) << ", " - << std::to_string(param.max_distance) << "]"; + ADEBUG << "(1) projection_dist = " << std::to_string(projection_dist) + << " [" << std::to_string(param.min_distance) << ", " + << std::to_string(param.max_distance) << "]"; if (projection_dist < param.min_distance || projection_dist > param.max_distance) { return -1; @@ -392,8 +231,8 @@ inline ScalarType Group::compute_distance(const Group& tar_group, if (norm_v >= kEpsilon) { departure_dist /= norm_v; } - XLOG(DEBUG) << "(2) departure_dist = " << std::to_string(departure_dist) - << " (" << std::to_string(param.max_departure_distance) << ")"; + ADEBUG << "(2) departure_dist = " << std::to_string(departure_dist) + << " (" << std::to_string(param.max_departure_distance) << ")"; if (departure_dist > param.max_departure_distance) { return -2; } @@ -401,33 +240,22 @@ inline ScalarType Group::compute_distance(const Group& tar_group, // (3) compute the deviation angle from reference marker to the target one // orientation angle of end markers on reference group ScalarType beta = this->end_angle; - XLOG(DEBUG) << "beta = " << std::to_string(beta / M_PI * 180.0); + ADEBUG << "beta = " << std::to_string(beta / M_PI * 180.0); // angle from reference marker to the target one ScalarType gamma = std::atan2(displacement(1), displacement(0)); - XLOG_IF(ERROR, gamma < -static_cast(M_PI) || - gamma > static_cast(M_PI)) - << "gamma is out range of [-pi, pi]: " << gamma; if (gamma < 0) { gamma += 2 * static_cast(M_PI); } - XLOG(DEBUG) << "gamma = " << std::to_string(gamma / M_PI * 180.0); + ADEBUG << "gamma = " << std::to_string(gamma / M_PI * 180.0); ScalarType deviation_angle_dist = std::abs(beta - gamma); - XLOG_IF(ERROR, deviation_angle_dist < 0 || - deviation_angle_dist > 2 * static_cast(M_PI)) - << "deviation_angle_dist is out range of [0, 2*pi]: " - << deviation_angle_dist; if (deviation_angle_dist > static_cast(M_PI)) { deviation_angle_dist = 2 * static_cast(M_PI) - deviation_angle_dist; } - XLOG_IF(ERROR, deviation_angle_dist < 0 || - deviation_angle_dist > static_cast(M_PI)) - << "deviation_angle_dist is out range of [0, pi]: " - << deviation_angle_dist; - XLOG(DEBUG) << "(3) deviation_angle_dist = " - << std::to_string(deviation_angle_dist / M_PI * 180.0); + ADEBUG << "(3) deviation_angle_dist = " + << std::to_string(deviation_angle_dist / M_PI * 180.0); if (deviation_angle_dist > param.max_deviation_angle) { return -3; } @@ -435,205 +263,26 @@ inline ScalarType Group::compute_distance(const Group& tar_group, // (4) relative orientation angle ScalarType tar_group_start_len = tar_group.start_orie.norm(); ScalarType orie_dist = 0; - XLOG(DEBUG) << "(4a) tar_group_start_len = " - << std::to_string(tar_group_start_len) << " (" - << std::to_string(param.min_orientation_estimation_size) << ")"; + ADEBUG << "(4a) tar_group_start_len = " + << std::to_string(tar_group_start_len) << " (" + << std::to_string(param.min_orientation_estimation_size) << ")"; if (tar_group_start_len > param.min_orientation_estimation_size) { // orientation angle of start markers on target group ScalarType alpha = tar_group.start_angle; - XLOG(DEBUG) << "alpha = " << std::to_string(alpha / M_PI * 180.0); - - orie_dist = std::abs(alpha - beta); - XLOG_IF(ERROR, - orie_dist < 0 || orie_dist > 2 * static_cast(M_PI)) - << "orie_dist is out range of [0, 2*pi]: " << orie_dist; - if (orie_dist > static_cast(M_PI)) { - orie_dist = 2 * static_cast(M_PI) - orie_dist; - } - XLOG_IF(ERROR, orie_dist < 0 || orie_dist > static_cast(M_PI)) - << "orie_dist is out range of [0, pi]: " << orie_dist; - XLOG(DEBUG) << "(4b) orie_dist = " - << std::to_string(orie_dist / M_PI * 180.0) << " (" - << std::to_string(param.max_relative_orie / M_PI * 180.0) - << ")"; - if (orie_dist > param.max_relative_orie) { - return -4; - } - } - - // - ScalarType r = - std::max(std::abs(param.min_distance), std::abs(param.max_distance)); - if (r > kEpsilon) { - projection_dist = std::abs(projection_dist) / r; - } - if (param.max_departure_distance > kEpsilon) { - departure_dist /= param.max_departure_distance; - } - if (param.max_deviation_angle > kEpsilon) { - deviation_angle_dist /= param.max_deviation_angle; - } - if (param.max_relative_orie > kEpsilon) { - orie_dist /= param.max_relative_orie; - } - - ScalarType dist = param.distance_weight * projection_dist + - param.departure_distance_weight * departure_dist + - param.deviation_angle_weight * deviation_angle_dist + - param.relative_orie_weight * orie_dist; - - XLOG_IF(ERROR, !std::isfinite(dist)) << "the distance value is infinite."; - XLOG_IF(ERROR, dist < 0) << "the distance value is negative: " - << std::to_string(dist); - - XLOG(DEBUG) << "dist = " << std::to_string(dist) << "\n"; - return dist; -} - -inline ScalarType Group::compute_distance( - const Group& tar_group, const AssociationParam& param, - int cur_group_end_marker_descend_id, - int tar_group_start_marker_ascend_id) const { - XLOG(DEBUG) << "cur_group_end_marker_descend_id=" - << cur_group_end_marker_descend_id; - XLOG_IF(ERROR, cur_group_end_marker_descend_id < 0) - << "cur_group_end_marker_descend_id = " - << cur_group_end_marker_descend_id; - XLOG_IF(ERROR, cur_group_end_marker_descend_id >= - static_cast(this->end_pos_list.size())) - << "cur_group_end_marker_descend_id = " - << cur_group_end_marker_descend_id; - - Vector2D cur_group_end_pos = - this->end_pos_list.at(cur_group_end_marker_descend_id); - Vector2D cur_group_end_orie = - this->end_orie_list.at(cur_group_end_marker_descend_id); - - XLOG(DEBUG) << "tar_group_start_marker_ascend_id=" - << tar_group_start_marker_ascend_id; - XLOG_IF(ERROR, tar_group_start_marker_ascend_id < 0) - << "tar_group_start_marker_ascend_id = " - << tar_group_start_marker_ascend_id; - XLOG_IF(ERROR, tar_group_start_marker_ascend_id >= - static_cast(tar_group.start_pos_list.size())) - << "tar_group_start_marker_ascend_id = " - << tar_group_start_marker_ascend_id; - - Vector2D tar_group_start_pos = - tar_group.start_pos_list.at(tar_group_start_marker_ascend_id); - Vector2D tar_group_start_orie = - tar_group.start_orie_list.at(tar_group_start_marker_ascend_id); - - XLOG(DEBUG) << "source point = (" << cur_group_end_pos(0) << ", " - << cur_group_end_pos(1) << ")"; - XLOG(DEBUG) << "target point = (" << tar_group_start_pos(0) << ", " - << tar_group_start_pos(1) << ")"; - Vector2D displacement = tar_group_start_pos - cur_group_end_pos; - ScalarType norm_v = cur_group_end_orie.norm(); - if (norm_v < kEpsilon) { - XLOG(INFO) - << "norm of orientation vector for reference group is too small: " - << norm_v; - } - - // (1) compute the projection distance of the target marker w.r.t. model - ScalarType projection_dist = displacement.dot(cur_group_end_orie); - if (norm_v >= kEpsilon) { - projection_dist /= norm_v; - } - XLOG(DEBUG) << "(1) projection_dist = " << std::to_string(projection_dist) - << " [" << std::to_string(param.min_distance) << ", " - << std::to_string(param.max_distance) << "]"; - if (projection_dist < param.min_distance || - projection_dist > param.max_distance) { - return -1; - } - - // (2) compute the departure distance to target marker - // line equation: - // A * x + B * y + C = 0 - // A = _v.y(), B = -_v.x(), C = _v.x() * _p.y() - _v.y() * _p.x(); - // the normal distance from a point (x2, y2) to the line: - // d = |A * x2 + B * y2 + C| / sqrt(A^2 + B^2) - ScalarType departure_dist = - std::abs(cur_group_end_orie.y() * tar_group_start_pos.x() - - cur_group_end_orie.x() * tar_group_start_pos.y() + - cur_group_end_orie.x() * cur_group_end_pos.y() - - cur_group_end_orie.y() * cur_group_end_pos.x()); - if (norm_v >= kEpsilon) { - departure_dist /= norm_v; - } - XLOG(DEBUG) << "(2) departure_dist = " << std::to_string(departure_dist) - << " (" << std::to_string(param.max_departure_distance) << ")"; - if (departure_dist > param.max_departure_distance) { - return -2; - } - - // (3) compute the deviation angle from reference marker to the target one - // orientation angle of end marker on reference group - ScalarType beta = this->end_angle_list.at(cur_group_end_marker_descend_id); - XLOG(DEBUG) << "beta = " << std::to_string(beta / M_PI * 180.0); - - // angle from reference marker to the target one - ScalarType gamma = std::atan2(displacement(1), displacement(0)); - XLOG_IF(ERROR, gamma < -static_cast(M_PI) || - gamma > static_cast(M_PI)) - << "gamma is out range of [-pi, pi]: " << gamma; - if (gamma < 0) { - gamma += 2 * static_cast(M_PI); - } - XLOG(DEBUG) << "gamma = " << std::to_string(gamma / M_PI * 180.0); - - ScalarType deviation_angle_dist = std::abs(beta - gamma); - XLOG_IF(ERROR, deviation_angle_dist < 0 || - deviation_angle_dist > 2 * static_cast(M_PI)) - << "deviation_angle_dist is out range of [0, 2*pi]: " - << deviation_angle_dist; - if (deviation_angle_dist > static_cast(M_PI)) { - deviation_angle_dist = - 2 * static_cast(M_PI) - deviation_angle_dist; - } - XLOG_IF(ERROR, deviation_angle_dist < 0 || - deviation_angle_dist > static_cast(M_PI)) - << "deviation_angle_dist is out range of [0, pi]: " - << deviation_angle_dist; - XLOG(DEBUG) << "(3) deviation_angle_dist = " - << std::to_string(deviation_angle_dist / M_PI * 180.0); - if (deviation_angle_dist > param.max_deviation_angle) { - return -3; - } - - // (4) relative orientation angle - ScalarType tar_group_start_len = tar_group_start_orie.norm(); - ScalarType orie_dist = 0; - XLOG(DEBUG) << "(4a) tar_group_start_len = " - << std::to_string(tar_group_start_len) << " (" - << std::to_string(param.min_orientation_estimation_size) << ")"; - if (tar_group_start_len > param.min_orientation_estimation_size) { - // orientation angle of start markers on target group - ScalarType alpha = - tar_group.start_angle_list.at(tar_group_start_marker_ascend_id); - XLOG(DEBUG) << "alpha = " << std::to_string(alpha / M_PI * 180.0); + ADEBUG << "alpha = " << std::to_string(alpha / M_PI * 180.0); orie_dist = std::abs(alpha - beta); - XLOG_IF(ERROR, - orie_dist < 0 || orie_dist > 2 * static_cast(M_PI)) - << "orie_dist is out range of [0, 2*pi]: " << orie_dist; if (orie_dist > static_cast(M_PI)) { orie_dist = 2 * static_cast(M_PI) - orie_dist; } - XLOG_IF(ERROR, orie_dist < 0 || orie_dist > static_cast(M_PI)) - << "orie_dist is out range of [0, pi]: " << orie_dist; - XLOG(DEBUG) << "(4b) orie_dist = " - << std::to_string(orie_dist / M_PI * 180.0) << " (" - << std::to_string(param.max_relative_orie / M_PI * 180.0) - << ")"; + ADEBUG << "(4b) orie_dist = " + << std::to_string(orie_dist / M_PI * 180.0) << " (" + << std::to_string(param.max_relative_orie / M_PI * 180.0) << ")"; if (orie_dist > param.max_relative_orie) { return -4; } } - // ScalarType r = std::max(std::abs(param.min_distance), std::abs(param.max_distance)); if (r > kEpsilon) { @@ -654,22 +303,17 @@ inline ScalarType Group::compute_distance( param.deviation_angle_weight * deviation_angle_dist + param.relative_orie_weight * orie_dist; - XLOG_IF(ERROR, !std::isfinite(dist)) << "the distance value is infinite."; - XLOG_IF(ERROR, dist < 0) << "the distance value is negative: " - << std::to_string(dist); - - XLOG(DEBUG) << "dist = " << std::to_string(dist) << "\n"; + ADEBUG << "overall distance = " << std::to_string(dist) << "\n"; return dist; } -inline Bbox Group::bbox(const std::vector& markers) const { +inline Bbox Group::GetBbox(const std::vector& markers) const { Bbox box(std::numeric_limits::max(), // x_min std::numeric_limits::max(), // y_min -std::numeric_limits::max(), // x_max -std::numeric_limits::max()); // y_max for (int i : this->marker_idx) { - assert(i >= 0 && i < static_cast(markers.size())); box(0) = std::min(box(0), markers[i].pos.x()); box(1) = std::min(box(1), markers[i].pos.y()); box(2) = std::max(box(2), markers[i].pos.x()); diff --git a/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/lane_frame.cc b/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/lane_frame.cc new file mode 100644 index 0000000000..9d0e0426d6 --- /dev/null +++ b/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/lane_frame.cc @@ -0,0 +1,968 @@ +/****************************************************************************** + * Copyright 2018 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include + +#include +#include + +#include "modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/lane_frame.h" + +namespace apollo { +namespace perception { + +using std::vector; +using std::shared_ptr; +using std::sort; +using std::unordered_map; +using std::unordered_set; +using std::pair; +using std::make_pair; +using std::string; + +/// for class LaneFrame +void LaneFrame::ComputeBbox() { + boxes_.clear(); + boxes_.reserve(graphs_.size()); + for (size_t k = 0; k < graphs_.size(); ++k) { + boxes_.push_back(Bbox()); + boxes_[k](0) = std::numeric_limits::max(); // x_min + boxes_[k](1) = std::numeric_limits::max(); // y_min + boxes_[k](2) = -std::numeric_limits::max(); // x_max + boxes_[k](3) = -std::numeric_limits::max(); // y_max + + for (size_t j = 0; j < graphs_[k].size(); ++j) { + int i = graphs_[k][j].first; + boxes_[k](0) = std::min(boxes_[k](0), markers_.at(i).pos(0)); + boxes_[k](1) = std::min(boxes_[k](1), markers_.at(i).pos(1)); + boxes_[k](2) = std::max(boxes_[k](2), markers_.at(i).pos(0)); + boxes_[k](3) = std::max(boxes_[k](3), markers_.at(i).pos(1)); + + if (markers_.at(i).shape_type != MarkerShapeType::POINT) { + boxes_[k](0) = std::min(boxes_[k](0), markers_.at(i).start_pos(0)); + boxes_[k](1) = std::min(boxes_[k](1), markers_.at(i).start_pos(1)); + boxes_[k](2) = std::max(boxes_[k](2), markers_.at(i).start_pos(0)); + boxes_[k](3) = std::max(boxes_[k](3), markers_.at(i).start_pos(1)); + } + } + } +} + +ScalarType LaneFrame::ComputeMarkerPairDistance(const Marker& ref, + const Marker& tar) { + ADEBUG << "compute distance between markers ..."; + ADEBUG << "reference marker: " + << "x=" << std::to_string(ref.pos(0)) << ", " + << "y=" << std::to_string(ref.pos(1)); + ADEBUG << "target marker: " + << "x=" << std::to_string(tar.start_pos(0)) << ", " + << "y=" << std::to_string(tar.start_pos(1)); + + ScalarType dist = static_cast(-1.0); + + Vector2D displacement; + switch (ref.shape_type) { + case MarkerShapeType::POINT: { + displacement = tar.pos - ref.pos; + break; + } + case MarkerShapeType::LINE_SEGMENT: { + displacement = tar.start_pos - ref.pos; + break; + } + default: { AFATAL << "unknown marker shape type."; } + } + + ScalarType pos_dist = static_cast(displacement.norm()); + ADEBUG << "pos_dist = " << std::to_string(pos_dist); + + // orientation angle of target marker + ScalarType alpha = tar.angle; + if (alpha < 0) { + alpha += (2 * static_cast(M_PI)); + } + ADEBUG << "alpha = " << std::to_string(alpha / M_PI * 180.0); + + // orientation angle of reference marker + ScalarType beta = ref.angle; + if (beta < 0) { + beta += (2 * static_cast(M_PI)); + } + ADEBUG << "beta = " << std::to_string(beta / M_PI * 180.0); + + // deviation angle from reference marker to the target one + ScalarType gamma = std::atan2(displacement(1), displacement(0)); + if (gamma < 0) { + gamma += (2 * static_cast(M_PI)); + } + ADEBUG << "gamma = " << std::to_string(gamma / M_PI * 180.0); + + ScalarType deviation_angle_dist = std::abs(beta - gamma); + if (deviation_angle_dist > static_cast(M_PI)) { + deviation_angle_dist = + 2 * static_cast(M_PI) - deviation_angle_dist; + } + ADEBUG << "deviation_angle_dist = " + << std::to_string(deviation_angle_dist / M_PI * 180.0); + + ScalarType orie_dist = std::abs(alpha - beta); + + if (orie_dist > static_cast(M_PI)) { + orie_dist = 2 * static_cast(M_PI) - orie_dist; + } + + ADEBUG << "orie_dist = " << std::to_string(orie_dist / M_PI * 180.0); + ADEBUG << "max_distance = " + << std::to_string(opts_.assoc_param.max_distance) << ", " + << "max_deviation_angle = " + << std::to_string( + opts_.assoc_param.max_deviation_angle / M_PI * 180.0) << ", " + << "max_relative_orie = " + << std::to_string( + opts_.assoc_param.max_relative_orie / M_PI * 180.0); + + if (pos_dist > opts_.assoc_param.max_distance || + deviation_angle_dist > opts_.assoc_param.max_deviation_angle || + orie_dist > opts_.assoc_param.max_relative_orie) { + return dist; + } + + pos_dist /= opts_.assoc_param.max_distance; + deviation_angle_dist /= opts_.assoc_param.max_deviation_angle; + orie_dist /= opts_.assoc_param.max_relative_orie; + + dist = opts_.assoc_param.distance_weight * pos_dist + + opts_.assoc_param.deviation_angle_weight * deviation_angle_dist + + opts_.assoc_param.relative_orie_weight * orie_dist; + return dist; +} + +bool LaneFrame::Init(const vector& input_cc, + const LaneFrameOptions& options) { + if (options.space_type != SpaceType::IMAGE) { + AERROR << "the space type is not IMAGE."; + return false; + } + opts_ = options; + + max_cc_num_ = static_cast(input_cc.size()); + markers_.clear(); + for (int i = 0; i < static_cast(input_cc.size()); ++i) { + const ConnectedComponentPtr cc_ptr = input_cc.at(i); + if (cc_ptr->GetPixelCount() >= opts_.min_cc_pixel_num && + (cc_ptr->bbox()->width() >= opts_.min_cc_size || + cc_ptr->bbox()->height() >= opts_.min_cc_size)) { + const shared_ptr>& inner_edges = + cc_ptr->GetInnerEdges(); + int n_inner_edges = static_cast(inner_edges->size()); + for (int j = 0; j < n_inner_edges; ++j) { + const ConnectedComponent::Edge* edge_ptr = &(inner_edges->at(j)); + Marker marker; + marker.shape_type = MarkerShapeType::LINE_SEGMENT; + marker.space_type = opts_.space_type; + marker.pos = cc_ptr->GetVertex(edge_ptr->end_vertex_id); + marker.image_pos = marker.pos; + marker.vis_pos = cv::Point(static_cast(marker.pos.x()), + static_cast(marker.pos.y())); + marker.start_pos = cc_ptr->GetVertex(edge_ptr->start_vertex_id); + marker.image_start_pos = marker.start_pos; + marker.vis_start_pos = + cv::Point(static_cast(marker.start_pos.x()), + static_cast(marker.start_pos.y())); + marker.angle = edge_ptr->orie; + if (marker.angle < -static_cast(M_PI) || + marker.angle > static_cast(M_PI)) { + AERROR << "marker.angle is out range of [-pi, pi]: " + << marker.angle; + return false; + } + + marker.orie(0) = std::cos(marker.angle); + marker.orie(1) = std::sin(marker.angle); + marker.original_id = static_cast(markers_.size()); + marker.cc_id = i; + marker.inner_edge_id = j; + marker.cc_edge_ascend_id = j; + marker.cc_edge_descend_id = n_inner_edges - 1 - j; + if (marker.cc_edge_descend_id < 0 || + marker.cc_edge_descend_id >= n_inner_edges) { + AERROR << "marker.cc_edge_descend_id = " + << marker.cc_edge_descend_id; + return false; + } + + if (!markers_.empty() && markers_.back().cc_id == i) { + markers_.back().cc_next_marker_id + = static_cast(markers_.size()); + } + markers_.push_back(marker); + } + } + } + + for (int i = 0; i < static_cast(markers_.size()); ++i) { + if (markers_[i].original_id != i) { + AERROR << "original marker id is not equal to " << i; + return false; + } + if (markers_[i].cc_id < 0) { + AERROR << "cc id is less than 0: " << markers_[i].cc_id; + return false; + } + if (markers_[i].inner_edge_id < 0) { + AERROR << "inner_edge_id is less than 0: " << markers_[i].inner_edge_id; + return false; + } + if (markers_[i].cc_edge_ascend_id < 0) { + AERROR << "cc_edge_ascend_id is less than 0: " + << markers_[i].cc_edge_ascend_id; + return false; + } + if (markers_[i].cc_edge_descend_id < 0) { + AERROR << "cc_edge_descend_id is less than 0: " + << markers_[i].cc_edge_descend_id; + return false; + } + + int i_next = markers_[i].cc_next_marker_id; + if (i_next != -1) { + if (i_next < 0) { + AERROR << "cc_next_marker_id is less than 0: " + << markers_[i].cc_next_marker_id; + return false; + } + if (i_next >= static_cast(markers_.size())) { + AERROR << "cc_next_marker_id is larger than marker size: " + << markers_[i].cc_next_marker_id; + return false; + } + + if (markers_[i_next].cc_id != markers_[i].cc_id) { + AERROR << "markers_[i_next].cc_id (" << markers_[i_next].cc_id << ") " + << "is not equal to markers_[i].cc_id (" << markers_[i].cc_id + << ")."; + return false; + } + if (markers_[i_next].cc_edge_ascend_id != + markers_[i].cc_edge_ascend_id + 1) { + AERROR << "markers_[i_next].cc_edge_ascend_id (" + << markers_[i_next].cc_edge_ascend_id << ") " + << "is not equal to markers_[i].cc_edge_ascend_id + 1 (" + << markers_[i].cc_edge_ascend_id + 1 << ")."; + return false; + } + if (markers_[i_next].cc_edge_descend_id + 1 != + markers_[i].cc_edge_descend_id) { + AERROR << "markers_[i_next].cc_edge_descend_id + 1 (" + << markers_[i_next].cc_edge_descend_id << ") " + << "is not equal to markers_[i].cc_edge_descend_id (" + << markers_[i].cc_edge_descend_id << ")."; + return false; + } + } + } + + is_projector_init_ = false; + + return true; +} + +bool LaneFrame::Init(const vector& input_cc, + const shared_ptr>& projector, + const LaneFrameOptions& options) { + if (options.space_type != SpaceType::VEHICLE) { + AERROR << "the space type is not VEHICLE."; + return false; + } + opts_ = options; + + projector_ = projector; + is_projector_init_ = true; + + max_cc_num_ = static_cast(input_cc.size()); + markers_.clear(); + for (int i = 0; i < static_cast(input_cc.size()); ++i) { + const ConnectedComponentPtr cc_ptr = input_cc.at(i); + ADEBUG << "cc " << i; + if (cc_ptr->GetPixelCount() >= opts_.min_cc_pixel_num) { + const shared_ptr>& inner_edges = + cc_ptr->GetInnerEdges(); + + int n = 0; + for (int j = 0; j < static_cast(inner_edges->size()); ++j) { + const ConnectedComponent::Edge* edge_ptr = &(inner_edges->at(j)); + Vector2D pos = cc_ptr->GetVertex(edge_ptr->end_vertex_id); + Vector2D start_pos = cc_ptr->GetVertex(edge_ptr->start_vertex_id); + + Marker marker; + marker.shape_type = MarkerShapeType::LINE_SEGMENT; + marker.space_type = opts_.space_type; + + if (!projector_->UvToXy(static_cast(pos(0)), + static_cast(pos(1)), + &(marker.pos))) { + continue; + } + marker.image_pos = pos; + if (projector_->is_vis()) { + if (!projector_->UvToXyImagePoint(static_cast(pos(0)), + static_cast(pos(1)), + &marker.vis_pos)) { + return false; + } + } + + if (!projector_->UvToXy(static_cast(start_pos(0)), + static_cast(start_pos(1)), + &(marker.start_pos))) { + continue; + } + marker.image_start_pos = start_pos; + if (projector_->is_vis()) { + if (!projector_->UvToXyImagePoint( + static_cast(start_pos(0)), + static_cast(start_pos(1)), + &marker.vis_start_pos)) { + return false; + } + } + + marker.orie = marker.pos - marker.start_pos; + marker.angle = std::atan2(marker.orie(1), marker.orie(0)); + if (marker.angle < -static_cast(M_PI) || + marker.angle > static_cast(M_PI)) { + AERROR << "marker.angle is out range of [-pi, pi]: " + << marker.angle; + return false; + } + + marker.orie(0) = std::cos(marker.angle); + marker.orie(1) = std::sin(marker.angle); + marker.original_id = static_cast(markers_.size()); + marker.cc_id = i; + marker.inner_edge_id = j; + marker.cc_edge_ascend_id = n++; + if (n <= 0 || n > static_cast(inner_edges->size())) { + AERROR << "n is out range of [0, " << inner_edges->size() << "]" + << ": " << n; + return false; + } + + if (!markers_.empty() && markers_.back().cc_id == i) { + markers_.back().cc_next_marker_id + = static_cast(markers_.size()); + } + markers_.push_back(marker); + + ADEBUG << "marker " << n << ": " + << "(" << marker.image_pos(0) << ", " << marker.image_pos(1) + << ")" + << " => " + << "(" << marker.pos(0) << ", " << marker.pos(1) << ")"; + } + + if (n > static_cast(inner_edges->size())) { + AERROR << "n is larger than inner edge size " + << inner_edges->size() << ": " << n; + return false; + } + + int m = static_cast(markers_.size()); + for (int j = 0; j < n; ++j) { + markers_[m - 1 - j].cc_edge_descend_id = j; + } + + bool is_small_cc = false; + if (n > 0) { + ScalarType x_min = std::numeric_limits::max(); + ScalarType x_max = -std::numeric_limits::max(); + ScalarType y_min = std::numeric_limits::max(); + ScalarType y_max = -std::numeric_limits::max(); + for (int j = m - n; j < m; ++j) { + x_min = std::min(x_min, markers_[j].start_pos(0)); + x_max = std::max(x_max, markers_[j].start_pos(0)); + y_min = std::min(y_min, markers_[j].start_pos(1)); + y_max = std::max(y_max, markers_[j].start_pos(1)); + + x_min = std::min(x_min, markers_[j].pos(0)); + x_max = std::max(x_max, markers_[j].pos(0)); + y_min = std::min(y_min, markers_[j].pos(1)); + y_max = std::max(y_max, markers_[j].pos(1)); + } + if (x_max - x_min < 0.5 && y_max - y_min < 0.5) { + AINFO << "x_min = " << x_min << ", " + << "x_max = " << x_max << ", " + << "width = " << x_max - x_min << ", " + << "y_min = " << y_min << ", " + << "y_max = " << y_max << ", " + << "height = " << y_max - y_min; + AINFO << "this cc is too small, ignore it."; + is_small_cc = true; + } + } + if (is_small_cc) { + markers_.resize(m - n); + } + } + } + + return true; +} + +// compute the candidate edges for each marker +vector LaneFrame::ComputeMarkerEdges( + vector>* edges) { + sort(markers_.begin(), markers_.end(), Marker::comp); + size_t tot_marker_num = markers_.size(); + + if (opts_.use_cc) { + cc_idx_.clear(); + cc_idx_.reserve(tot_marker_num); + cc_marker_lut_.clear(); + for (int i = 0; i < static_cast(tot_marker_num); ++i) { + cc_idx_.push_back(markers_[i].cc_id); + if (cc_marker_lut_.find(markers_[i].cc_id) == cc_marker_lut_.end()) { + cc_marker_lut_[markers_[i].cc_id] = vector(); + } + cc_marker_lut_[markers_[i].cc_id].push_back(i); + } + } + + // connection status: (-2): to be determined + // (-1): ended + // (id >= 0): determined + vector connect_status(tot_marker_num, -2); + + edges->clear(); // candidate edges for each marker + edges->resize(tot_marker_num); + int tot_num_edges = 0; + for (int i = 0; i < static_cast(tot_marker_num); ++i) { + edges->at(i).clear(); + + ADEBUG << "marker " << i << " cc " << markers_[i].cc_id + << " (" << markers_[i].cc_edge_descend_id << ")" + << " y=" << std::to_string(markers_[i].pos(1)); // y + + bool early_stop_flag = false; + if (opts_.use_cc) { + for (int j : cc_marker_lut_[markers_[i].cc_id]) { + if (markers_[j].cc_edge_ascend_id == + markers_[i].cc_edge_ascend_id + 1) { + (*edges)[i][j] = static_cast(0); + connect_status[i] = j; + ADEBUG << " dist " << j << " = " << 0.0 << " cc " + << markers_[i].cc_id << " ( << " + << markers_[j].cc_edge_descend_id << ")"; + break; + } + } + if (markers_[i].cc_edge_descend_id >= opts_.max_cc_marker_match_num) { + early_stop_flag = true; + } + } + + if (early_stop_flag) { + ADEBUG << " early stopped."; + continue; + } + + ScalarType y_thresh = 0; + switch (opts_.space_type) { + case SpaceType::IMAGE: { + y_thresh = markers_[i].pos(1) + opts_.min_y_search_offset; + break; + } + case SpaceType::VEHICLE: { + y_thresh = markers_[i].pos(0) - opts_.min_y_search_offset; + break; + } + default: { + AFATAL << "Error: unknown space type " << opts_.space_type; + } + } + + for (int j = 0; j < static_cast(tot_marker_num); ++j) { + // ignore the markers below marker i or itself + if (j == i) { + continue; + } + + if ((opts_.space_type == SpaceType::IMAGE && + markers_[j].start_pos(1) > y_thresh) || + (opts_.space_type == SpaceType::VEHICLE && + markers_[j].start_pos(0) < y_thresh)) { + continue; + } + + // ignore the markers in the same CC or the ones rather than + // the beginning markers + if (opts_.use_cc && + (markers_[j].cc_id == markers_[i].cc_id || + markers_[j].cc_edge_ascend_id >= opts_.max_cc_marker_match_num)) { + continue; + } + + ADEBUG << "marker i: y=" << std::to_string(markers_[i].pos(1)) + << ", " + << "marker j: y=" << std::to_string(markers_[j].start_pos(1)); + + ScalarType dist = ComputeMarkerPairDistance(markers_[i], markers_[j]); + ADEBUG << " dist " << j << " = " << std::to_string(dist) << " cc " + << markers_[j].cc_id << " ( << " + << markers_[j].cc_edge_ascend_id << ")"; + + if (dist >= 0) { + (*edges)[i][j] = dist; + } + } + + if ((*edges)[i].empty()) { + connect_status[i] = -1; + } + + tot_num_edges += static_cast(edges->at(i).size()); + } + + return connect_status; +} + +// greedy select connections based on marker groups +bool LaneFrame::GreedyGroupConnectAssociation() { + if (!opts_.use_cc) { + AERROR << "requires to use CC option."; + return false; + } + if (opts_.group_param.max_group_prediction_marker_num > + MAX_GROUP_PREDICTION_MARKER_NUM) { + AERROR << "max_group_prediction_marker_num is larger than " + << MAX_GROUP_PREDICTION_MARKER_NUM; + return false; + } + + if (opts_.orientation_estimation_skip_marker_num >= + opts_.group_param.max_group_prediction_marker_num) { + AERROR << "orientation_estimation_skip_marker_num (" + << opts_.orientation_estimation_skip_marker_num + << ") should be smaller than max_group_prediction_marker_num (" + << opts_.group_param.max_group_prediction_marker_num << ")"; + return false; + } + opts_.group_param.orientation_estimation_skip_marker_num = + opts_.orientation_estimation_skip_marker_num; + + AINFO << "max_group_prediction_marker_num = " + << opts_.group_param.max_group_prediction_marker_num; + AINFO << "orientation_estimation_skip_marker_num = " + << opts_.group_param.orientation_estimation_skip_marker_num; + + // generate marker groups based on CC heuristic + vector groups; + groups.reserve(max_cc_num_); + unordered_map hash_cc_idx(max_cc_num_); + for (int i = 0; i < static_cast(markers_.size()); ++i) { + if (hash_cc_idx.find(markers_[i].cc_id) == hash_cc_idx.end()) { + hash_cc_idx[markers_[i].cc_id] = static_cast(groups.size()); + groups.push_back(Group()); + groups.back().shape_type = markers_[i].shape_type; + groups.back().space_type = markers_[i].space_type; + if (groups.back().space_type != opts_.space_type) { + AERROR << "space type does not match."; + } + + groups.back().start_marker_idx.resize( + opts_.group_param.max_group_prediction_marker_num, -1); + groups.back().end_marker_idx.resize( + opts_.group_param.max_group_prediction_marker_num, -1); + } + + int j = hash_cc_idx[markers_[i].cc_id]; + groups[j].marker_idx.push_back(i); + + if (markers_[i].cc_edge_ascend_id < + opts_.group_param.max_group_prediction_marker_num) { + groups[j].start_marker_idx[markers_[i].cc_edge_ascend_id] = i; + } + if (markers_[i].cc_edge_descend_id < + opts_.group_param.max_group_prediction_marker_num) { + groups[j].end_marker_idx[markers_[i].cc_edge_descend_id] = i; + } + } + AINFO << "number of marker groups = " << groups.size(); + + // compute the orientation of starting and end points for each group + for (auto it_group = groups.begin(); it_group != groups.end(); ++it_group) { + int i_start = it_group->start_marker_idx[0]; + if (!IsValidMarker(i_start)) { + AERROR << "marker " << i_start << " is not valid."; + return false; + } + it_group->start_pos = + (markers_[i_start].shape_type == MarkerShapeType::POINT) + ? markers_[i_start].pos + : markers_[i_start].start_pos; + + int i_end = it_group->end_marker_idx[0]; + if (!IsValidMarker(i_end)) { + AERROR << "marker " << i_end << " is not valid."; + return false; + } + it_group->end_pos = markers_[i_end].pos; + + int k = it_group->ComputeOrientation(markers_, opts_.group_param); + it_group->start_marker_idx.resize(k); + it_group->end_marker_idx.resize(k); + } + + // compute distance matrix of marker groups + int n = static_cast(groups.size()); + Eigen::Matrix dist_mat = + Eigen::Matrix::Ones(n, n) * + static_cast(-1); + Eigen::Matrix connect_mat = + Eigen::Matrix::Zero(n, n); + vector> edges; // (linear id, distance value) + edges.reserve(size_t(n) * size_t(n)); + vector> to_group_idx(n); + vector> from_group_idx(n); + + for (int k = 0; k < n; ++k) { + const Group* cur_group = &(groups[k]); + + ScalarType y_thresh = 0; + switch (cur_group->space_type) { + case SpaceType::IMAGE: { + y_thresh = cur_group->end_pos(1) + opts_.min_y_search_offset; + break; + } + case SpaceType::VEHICLE: { + y_thresh = cur_group->end_pos(0) - opts_.min_y_search_offset; + break; + } + default: { + AFATAL << "unknown space type " << cur_group->space_type; + } + } + + to_group_idx[k].reserve(n); + from_group_idx[k].reserve(n); + for (int k1 = 0; k1 < n; ++k1) { + const Group* tar_group = &(groups[k1]); + + if (k1 == k) { + continue; + } + + if ((tar_group->space_type == SpaceType::IMAGE && + tar_group->start_pos(1) > y_thresh) || + (tar_group->space_type == SpaceType::VEHICLE && + tar_group->start_pos(0) < y_thresh)) { + continue; + } + + dist_mat(k, k1) = + cur_group->ComputeDistance(*tar_group, opts_.assoc_param); + if (dist_mat(k, k1) >= 0) { + connect_mat(k, k1) = 1; // 1: available to be connected + edges.push_back(make_pair(dist_mat(k, k1), k * n + k1)); + to_group_idx[k].insert(k1); + from_group_idx[k1].insert(k); + } + } + } + + // greedy select connections for group association + sort(edges.begin(), edges.end()); + for (auto it_edge = edges.begin(); it_edge != edges.end(); ++it_edge) { + // select the lowest-cost one from candidate edges + int src_group_id = it_edge->second / n; + int tar_group_id = it_edge->second % n; + if (std::abs(dist_mat(src_group_id, tar_group_id) - it_edge->first) >= + kEpsilon) { + AERROR << "the distance measure from group " << src_group_id + << " to group " << tar_group_id << " is not equal to " + << it_edge->first; + return false; + } + + if (connect_mat(src_group_id, tar_group_id) == 0) { + continue; + } + + // connect source group with target group and + // delete the candidate edges related to the selected one + for (int i = 0; i < n; ++i) { + connect_mat(src_group_id, i) = 0; + connect_mat(i, tar_group_id) = 0; + } + connect_mat(src_group_id, tar_group_id) = 1; + + to_group_idx[src_group_id].clear(); + to_group_idx[src_group_id].insert(tar_group_id); + from_group_idx[tar_group_id].clear(); + from_group_idx[tar_group_id].insert(src_group_id); + } + + // generate lane clusters + vector lut_to_id(n, -1); + vector lut_from_id(n, -1); + for (int i = 0; i < n; ++i) { + for (int j = 0; j < n; ++j) { + if (connect_mat(i, j) == 1) { + if (lut_to_id[i] != -1) { + AERROR << "lut_to_id of group " << i + << " is not equal to " << -1; + return false; + } + lut_to_id[i] = j; + + if (lut_from_id[j] != -1) { + AERROR << "lut_from_id of group " << j + << " is not equal to " << -1; + return false; + } + lut_from_id[j] = i; + + break; + } + } + } + + unordered_set hash_group_idx(n); + unordered_set hash_marker_idx(markers_.size()); + int cur_group_id = -1; + for (int k = 0; k < n; ++k) { + if (lut_from_id[k] == -1) { + // generate a new cluster + graphs_.push_back(Graph()); + cur_group_id = k; + + while (cur_group_id != -1) { + hash_group_idx.insert(cur_group_id); + + int n_markers_group = + static_cast(groups[cur_group_id].marker_idx.size()); + if (lut_to_id[cur_group_id] == -1) { + if (n_markers_group >= + opts_.orientation_estimation_skip_marker_num + 2) { + AddGroupIntoGraph( + groups[cur_group_id], + opts_.orientation_estimation_skip_marker_num, 0, + &(graphs_.back()), &hash_marker_idx); + } else { + AddGroupIntoGraph(groups[cur_group_id], + &(graphs_.back()), &hash_marker_idx); + } + } else { + if (n_markers_group >= + opts_.orientation_estimation_skip_marker_num * 2 + 2) { + AddGroupIntoGraph(groups[cur_group_id], + opts_.orientation_estimation_skip_marker_num, + opts_.orientation_estimation_skip_marker_num, + &(graphs_.back()), &hash_marker_idx); + } else { + AddGroupIntoGraph(groups[cur_group_id], + &(graphs_.back()), &hash_marker_idx); + } + } + cur_group_id = lut_to_id[cur_group_id]; + } + } + } + + if (static_cast(hash_group_idx.size()) != n) { + AERROR << "total number of groups is not equal to " << n; + return false; + } + if (hash_marker_idx.size() > markers_.size()) { + AERROR << "total number of markers is larger than " << markers_.size(); + return false; + } + + return true; +} + +int LaneFrame::AddGroupIntoGraph(const Group& group, + Graph* graph, + unordered_set* hash_marker_idx) { + int count_markers = 0; + + int cur_marker_id = group.start_marker_idx[0]; + if (!graph->empty() && graph->back().second == -1) { + if (cur_marker_id != -1) { + graph->back().second = cur_marker_id; + } + } + + int next_marker_id = -1; + while (cur_marker_id != -1) { + CHECK(IsValidMarker(cur_marker_id)); + CHECK(hash_marker_idx->find(cur_marker_id) == hash_marker_idx->end()); + + next_marker_id = markers_[cur_marker_id].cc_next_marker_id; + graph->push_back(make_pair(cur_marker_id, next_marker_id)); + hash_marker_idx->insert(cur_marker_id); + + cur_marker_id = next_marker_id; + ++count_markers; + } + + return count_markers; +} + +int LaneFrame::AddGroupIntoGraph(const Group& group, + const int& start_marker_ascend_id, + const int& end_marker_descend_id, + Graph* graph, + unordered_set* hash_marker_idx) { + int count_markers = 0; + + int cur_marker_id = group.start_marker_idx.at(start_marker_ascend_id); + if (!graph->empty() && graph->back().second == -1) { + if (cur_marker_id != -1) { + graph->back().second = cur_marker_id; + } + } + + int next_marker_id = -1; + while (cur_marker_id != -1 && + markers_[cur_marker_id].cc_edge_descend_id >= end_marker_descend_id) { + CHECK(IsValidMarker(cur_marker_id)); + CHECK(hash_marker_idx->find(cur_marker_id) == hash_marker_idx->end()); + + next_marker_id = markers_[cur_marker_id].cc_next_marker_id; + graph->push_back(make_pair(cur_marker_id, next_marker_id)); + hash_marker_idx->insert(cur_marker_id); + + cur_marker_id = next_marker_id; + ++count_markers; + } + if (!graph->empty()) { + graph->back().second = -1; // set the last node available to be connected + } + + return count_markers; +} + +bool LaneFrame::FitPolyCurve(const int& graph_id, + const ScalarType& graph_siz, + PolyModel* poly_coef, + ScalarType* lateral_distance) const { + if (poly_coef == nullptr) { + AERROR << "poly_coef is a null pointer."; + return false; + } + if (lateral_distance == nullptr) { + AERROR << "lateral_distance is a null pointer."; + return false; + } + + if (graph_id < 0 || graph_id >= static_cast(graphs_.size())) { + AERROR << "graph " << graph_id << "is out range of [0, " + << graphs_.size() << ")."; + return false; + } + + const Graph& graph = graphs_[graph_id]; + + vector pos_data; + pos_data.reserve(graph.size() * 2); + for (auto it = graph.begin(); it != graph.end(); ++it) { + int i = it->first; + if (i < 0 || i >= static_cast(markers_.size())) { + AERROR << "marker " << i << "is out range of [0, " << markers_.size() + << ")."; + return false; + } + + if (markers_[i].shape_type != MarkerShapeType::POINT) { + pos_data.push_back(markers_[i].start_pos); + } + pos_data.push_back(markers_[i].pos); + } + pos_data.shrink_to_fit(); + + if (pos_data.size() == 1) { + AERROR << "only one point in graph " << graph_id; + return false; + } + + int order = 0; + if (pos_data.size() < 3 || graph_siz < opts_.max_size_to_fit_straight_line) { + order = 1; // fit a 1st-order polynomial curve (straight line) + } else { + order = 2; // fit a 2nd-order polynomial curve + } + + if (!PolyFit(pos_data, order, poly_coef)) { + AERROR << "failed to fit " << order << " order polynomial curve."; + return false; + } + + *lateral_distance = (*poly_coef)(0); + + ADEBUG << "succeed to fit a " << order << " order polynomial curve: " + << "lateral distance = " << *lateral_distance; + return true; +} + +bool LaneFrame::Process(LaneInstancesPtr instances) { + if (instances == NULL) { + AERROR << "the pointer of input instances is null."; + return false; + } + + // do marker association + switch (opts_.assoc_param.method) { + case AssociationMethod::GREEDY_GROUP_CONNECT: { + AINFO << "using greedy group connection algorithm " + << "for marker association ..."; + if (!GreedyGroupConnectAssociation()) { + AERROR << "failed to do marker association."; + return false; + } + break; + } + default: { + AFATAL << "unknown marker association method."; + } + } + AINFO << "number of lane instance candidates = " << graphs_.size(); + + // compute tight bounding box for graphs + ComputeBbox(); + + // generate lane instances + instances->clear(); + instances->reserve(graphs_.size()); + for (int k = 0; k < GraphNum(); ++k) { + ScalarType siz = std::max(boxes_.at(k)(2) - boxes_.at(k)(0), + boxes_.at(k)(3) - boxes_.at(k)(1)); + // remove too small graphs + if (siz >= opts_.min_instance_size_prefiltered) { + instances->push_back(LaneInstance(k, siz, boxes_.at(k))); + } else { + ADEBUG << "size of graph " << k << " is too small: " << siz << " (" + << opts_.min_instance_size_prefiltered << "), " + << "removed."; + } + } + + instances->shrink_to_fit(); + return true; +} + +} // namespace perception +} // namespace apollo diff --git a/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/lane_frame.h b/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/lane_frame.h new file mode 100644 index 0000000000..0d1f9dc87b --- /dev/null +++ b/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/lane_frame.h @@ -0,0 +1,181 @@ +/****************************************************************************** + * Copyright 2018 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +// @brief: lane detection on a single image frame + +#ifndef MODULES_PERCEPTION_OBSTACLE_CAMERA_CC_LANE_POST_PROCESSOR_FRAME_H_ +#define MODULES_PERCEPTION_OBSTACLE_CAMERA_CC_LANE_POST_PROCESSOR_FRAME_H_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "modules/common/log.h" +#include "modules/perception/obstacle/camera/lane_post_process/common/type.h" +#include "modules/perception/obstacle/camera/lane_post_process/common/util.h" +#include "modules/perception/obstacle/camera/lane_post_process/common/connected_component.h" +#include "modules/perception/obstacle/camera/lane_post_process/common/projector.h" +#include "modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/group.h" + +namespace apollo { +namespace perception { + +#define MAX_GRAPH_NUM 20 + +// #ifndef _DUMP_RESULT +// #define _DUMP_RESULT true +// #endif + +struct LaneFrameOptions { + // used for initialization + SpaceType space_type; // space type + cv::Rect image_roi; + bool use_cc; + int min_cc_pixel_num; // minimum number of pixels of CC + int min_cc_size; // minimum size of CC + + // used for greedy search association method + int max_cc_marker_match_num; // maximum number of markers used for matching + // for each CC + + // used for marker association + ScalarType min_y_search_offset; // minimum longitudinal offset used for + // search upper markers + AssociationParam assoc_param; + GroupParam group_param; + int orientation_estimation_skip_marker_num; + + // for determining lane object label + ScalarType lane_interval_distance; // predefined label interval distance + + // for fitting curve + ScalarType min_instance_size_prefiltered; // minimum size of lane instance to + // be prefiltered + ScalarType max_size_to_fit_straight_line; // maximum size of instance to fit + // a straight line + + LaneFrameOptions() + : space_type(SpaceType::IMAGE), + use_cc(true), + min_cc_pixel_num(10), + min_cc_size(5), + max_cc_marker_match_num(1), + min_y_search_offset(0.0), + orientation_estimation_skip_marker_num(1), + lane_interval_distance(4.0), + min_instance_size_prefiltered(3.0), + max_size_to_fit_straight_line(5.0) {} +}; + +class LaneFrame { + public: + LaneFrame() {} + ~LaneFrame() {} + + bool Init(const std::vector& input_cc, + const LaneFrameOptions& options); + + bool Init(const std::vector& input_cc, + const std::shared_ptr>& projector, + const LaneFrameOptions& options); + + void SetTransformer( + const std::shared_ptr>& projector) { + projector_ = projector; + is_projector_init_ = true; + } + + bool Process(LaneInstancesPtr instances); + + int MarkerNum() const { + return static_cast(markers_.size()); + } + + bool IsValidMarker(int i) { + return i >= 0 && i < MarkerNum(); + } + + const Marker* marker(int i) { + return &(markers_.at(i)); + } + + int GraphNum() const { + return static_cast(graphs_.size()); + } + + const Graph* graph(int i) { + return &(graphs_.at(i)); + } + + Bbox bbox(int i) const { + return boxes_.at(i); + } + + bool FitPolyCurve(const int& graph_id, const ScalarType& graph_siz, + PolyModel* poly_coef, ScalarType* lateral_distance) const; + + protected: + ScalarType ComputeMarkerPairDistance(const Marker& ref, const Marker& tar); + + std::vector ComputeMarkerEdges( + std::vector>* edges); + + bool GreedyGroupConnectAssociation(); + + int AddGroupIntoGraph(const Group& group, Graph* graph, + std::unordered_set* hash_marker_idx); + + int AddGroupIntoGraph(const Group& group, + const int& start_marker_ascend_id, + const int& end_marker_descend_id, + Graph* graph, + std::unordered_set* hash_marker_idx); + + void ComputeBbox(); + + private: + // options + LaneFrameOptions opts_; + + // markers + std::vector markers_; + int max_cc_num_; + + // CC index for each marker + std::vector cc_idx_; + // marker indices for each CC + std::unordered_map> cc_marker_lut_; + + std::shared_ptr> projector_; + bool is_projector_init_; + + // lane marker clusters + std::vector graphs_; + // tight bounding boxes of lane clusters + std::vector boxes_; +}; + +} // namespace perception +} // namespace apollo + +#endif // MODULES_PERCEPTION_OBSTACLE_CAMERA_CC_LANE_POST_PROCESSOR_FRAME_H_ diff --git a/modules/perception/obstacle/camera/lane_post_process/common/BUILD b/modules/perception/obstacle/camera/lane_post_process/common/BUILD index 716f141a34..82c174d47d 100644 --- a/modules/perception/obstacle/camera/lane_post_process/common/BUILD +++ b/modules/perception/obstacle/camera/lane_post_process/common/BUILD @@ -33,4 +33,16 @@ cc_library( ], ) +cc_library( + name = "projector", + hdrs = ["projector.h"], + deps = [ + "//modules/common:log", + "//modules/perception/lib/config_manager:config_manager", + "//modules/perception/obstacle/camera/common:obstacle_camera_common", + "@eigen//:eigen", + "@opencv2//:core", + ], +) + cpplint() diff --git a/modules/perception/obstacle/camera/lane_post_process/common/connected_component.cc b/modules/perception/obstacle/camera/lane_post_process/common/connected_component.cc index 078b2fb42d..577b6c9c0f 100644 --- a/modules/perception/obstacle/camera/lane_post_process/common/connected_component.cc +++ b/modules/perception/obstacle/camera/lane_post_process/common/connected_component.cc @@ -14,8 +14,6 @@ * limitations under the License. *****************************************************************************/ -// @brief: connected component analysis for lane detection - #include #include #include diff --git a/modules/perception/obstacle/camera/lane_post_process/common/projector.h b/modules/perception/obstacle/camera/lane_post_process/common/projector.h index 840a2730ee..dd18f822e5 100644 --- a/modules/perception/obstacle/camera/lane_post_process/common/projector.h +++ b/modules/perception/obstacle/camera/lane_post_process/common/projector.h @@ -1,5 +1,5 @@ /****************************************************************************** - * Copyright 2017 The Apollo Authors. All Rights Reserved. + * Copyright 2018 The Apollo Authors. All Rights Reserved. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -14,15 +14,18 @@ * limitations under the License. *****************************************************************************/ -// brief: image space to ego-car ground space projection +// brief: image space to ego-car vehicle space projection -#ifndef MODULES_PERCEPTION_OBSTACLE_CAMERA_LANE_POST_PROCESS_COMMON_PROJECTOR_H_ -#define MODULES_PERCEPTION_OBSTACLE_CAMERA_LANE_POST_PROCESS_COMMON_PROJECTOR_H_ +#ifndef MODULES_PERCEPTION_OBSTACLE_CAMERA_LANE_POST_PROCESS_PROJECTOR_H_ +#define MODULES_PERCEPTION_OBSTACLE_CAMERA_LANE_POST_PROCESS_PROJECTOR_H_ -#include #include #include +#include +#include +#include + #include "modules/common/log.h" #include "modules/perception/lib/config_manager/calibration_config_manager.h" #include "modules/perception/obstacle/camera/common/util.h" @@ -30,366 +33,330 @@ namespace apollo { namespace perception { -//#define _DEBUG false - template class Projector { public: Projector(); - bool init(const cv::Rect &roi, const T &max_distance = 200.0, + bool Init(const cv::Rect &roi, const T &max_distance = 200.0, bool visualize = false); - bool uv_2_xy(const T &u, const T &v, Eigen::Matrix *p) const; + bool UvToXy(const T &u, const T &v, Eigen::Matrix *p) const; - bool uv_2_xy(const int &u, const int &v, Eigen::Matrix *p) const { - return uv_2_xy(static_cast(u), static_cast(v), p); + bool UvToXy(const int &u, const int &v, Eigen::Matrix *p) const { + return UvToXy(static_cast(u), static_cast(v), p); } - Eigen::Affine3d _camera_ex; - Eigen::Affine3d _camera_to_ego_car; - - bool is_init() const { - return _is_init; - } + bool is_init() const { return is_init_; } - bool is_valid_uv(const int &x, const int &y) const { - return is_valid_uv(static_cast(x), static_cast(y)); + bool IsValidUv(const int &x, const int &y) const { + return IsValidUv(static_cast(x), static_cast(y)); } - bool is_valid_uv(const T &x, const T &y) const; + bool IsValidUv(const T &x, const T &y) const; - bool is_valid_xy(const T &x, const T &y) const { - return x >= _xy_xmin && x <= _xy_xmax && y >= _xy_ymin && y <= _xy_ymax; + bool IsValidXy(const T &x, const T &y) const { + return x >= xy_xmin_ && x <= xy_xmax_ && y >= xy_ymin_ && y <= xy_ymax_; } - bool is_vis() const { - return _is_vis; - } - T x_step() const { - return _x_step; - } - T y_step() const { - return _y_step; - } - T xy_image_xmin() const { - return _xy_image_xmin; - } - T xy_image_xmax() const { - return _xy_image_xmax; - } - T xy_image_ymin() const { - return _xy_image_ymin; - } - T xy_image_ymax() const { - return _xy_image_ymax; - } + bool is_vis() const { return is_vis_; } + T x_step() const { return x_step_; } + T y_step() const { return y_step_; } + T xy_image_xmin() const { return xy_image_xmin_; } + T xy_image_xmax() const { return xy_image_xmax_; } + T xy_image_ymin() const { return xy_image_ymin_; } + T xy_image_ymax() const { return xy_image_ymax_; } - bool is_valid_xy_in_xy_image(const T &x, const T &y) const { - return x >= _xy_image_xmin && x <= _xy_image_xmax && y >= _xy_image_ymin && - y <= _xy_image_ymax; + bool IsValidXyInXyImage(const T &x, const T &y) const { + return x >= xy_image_xmin_ && x <= xy_image_xmax_ && + y >= xy_image_ymin_ && y <= xy_image_ymax_; } - int xy_image_cols() const { - CHECK(_is_init) << "Error: projector is not initialized."; - return _xy_image_cols; - } + int xy_image_cols() const { return xy_image_cols_; } - int xy_image_rows() const { - CHECK(_is_init) << "Error: projector is not initialized."; - return _xy_image_rows; - } + int xy_image_rows() const { return xy_image_rows_; } - bool xy_2_xy_image_point(const T &x, const T &y, cv::Point *p) const; - bool xy_2_xy_image_point(const Eigen::Matrix &pos, - cv::Point *p) const { - return xy_2_xy_image_point(pos.x(), pos.y(), p); + bool XyToXyImagePoint(const T &x, const T &y, cv::Point *p) const; + bool XyToXyImagePoint(const Eigen::Matrix &pos, + cv::Point *p) const { + return XyToXyImagePoint(pos.x(), pos.y(), p); } - bool uv_2_xy_image_point(const T &u, const T &v, cv::Point *p) const; + bool UvToXyImagePoint(const T &u, const T &v, cv::Point *p) const; - bool uv_2_xy_image_point(const int &u, const int &v, cv::Point *p) const { - return uv_2_xy_image_point(static_cast(u), static_cast(v), p); + bool UvToXyImagePoint(const int &u, const int &v, cv::Point *p) const { + return UvToXyImagePoint(static_cast(u), static_cast(v), p); } protected: - bool project(T u, T v, Eigen::Matrix *xy_point); + bool Project(const T &u, const T &v, Eigen::Matrix *xy_point); private: - Eigen::Matrix _trans_mat; + Eigen::Matrix trans_mat_; // for visualizing IPM image - std::vector> _xy_grid; - std::vector _uv_2_xy_image; - T _x_step; - T _y_step; - T _xy_image_xmin; - T _xy_image_xmax; - int _xy_image_cols; - T _xy_image_ymin; - T _xy_image_ymax; - int _xy_image_rows; + std::vector> xy_grid_; + std::vector uv_2_xy_image_; + T x_step_; + T y_step_; + T xy_image_xmin_; + T xy_image_xmax_; + int xy_image_cols_; + T xy_image_ymin_; + T xy_image_ymax_; + int xy_image_rows_; // ROI limits in IPM space - T _xy_xmin; - T _xy_ymin; - T _xy_xmax; - T _xy_ymax; + T xy_xmin_; + T xy_ymin_; + T xy_xmax_; + T xy_ymax_; // ROI bounds in image space - T _uv_xmin; - T _uv_ymin; - T _uv_xmax; - T _uv_ymax; + T uv_xmin_; + T uv_ymin_; + T uv_xmax_; + T uv_ymax_; - T _uv_roi_x_step; // the horizontal direction sampling step of ROI in image + T uv_roi_x_step_; // the horizontal direction sampling step of ROI in image // space - T _uv_roi_y_step; // the vertical direction sampling step of ROI in image + T uv_roi_y_step_; // the vertical direction sampling step of ROI in image // space - int _uv_roi_cols; // the column number of ROI in image space - int _uv_roi_rows; // the row number of ROI in image space - int _uv_roi_count; // the count of ROI sample points + int uv_roi_cols_; // the column number of ROI in image space + int uv_roi_rows_; // the row number of ROI in image space + int uv_roi_count_; // the count of ROI sample points - std::vector _valid_xy; - std::vector> _uv_2_xy; + std::vector valid_xy_; + std::vector> uv_2_xy_; - bool _is_init; - bool _is_vis; + bool is_init_; + bool is_vis_; }; template Projector::Projector() { - _xy_xmin = std::numeric_limits::max(); - _xy_ymin = std::numeric_limits::max(); - _xy_xmax = -std::numeric_limits::max(); - _xy_ymax = -std::numeric_limits::max(); + xy_xmin_ = std::numeric_limits::max(); + xy_ymin_ = std::numeric_limits::max(); + xy_xmax_ = -std::numeric_limits::max(); + xy_ymax_ = -std::numeric_limits::max(); - _uv_xmin = static_cast(0); - _uv_ymin = static_cast(0); - _uv_xmax = static_cast(0); - _uv_ymax = static_cast(0); + uv_xmin_ = static_cast(0); + uv_ymin_ = static_cast(0); + uv_xmax_ = static_cast(0); + uv_ymax_ = static_cast(0); - _uv_roi_x_step = static_cast(1); - _uv_roi_y_step = static_cast(1); + uv_roi_x_step_ = static_cast(1); + uv_roi_y_step_ = static_cast(1); - _uv_roi_cols = static_cast((_uv_xmax - _uv_xmin) / _uv_roi_x_step) + 1; - _uv_roi_rows = static_cast((_uv_ymax - _uv_ymin) / _uv_roi_y_step) + 1; - _uv_roi_count = _uv_roi_rows * _uv_roi_cols; + uv_roi_cols_ = static_cast((uv_xmax_ - uv_xmin_) / uv_roi_x_step_) + 1; + uv_roi_rows_ = static_cast((uv_ymax_ - uv_ymin_) / uv_roi_y_step_) + 1; + uv_roi_count_ = uv_roi_rows_ * uv_roi_cols_; - _is_vis = false; - _x_step = static_cast(0.1); - _y_step = static_cast(0.1); + is_vis_ = false; + x_step_ = static_cast(0.1); + y_step_ = static_cast(0.1); - _is_init = false; + is_init_ = false; } template -bool Projector::init(const cv::Rect &roi, const T &max_distance, +bool Projector::Init(const cv::Rect &roi, const T &max_distance, bool visualize) { AINFO << "Initialize projector ..."; // read transformation matrix from calibration config manager - config_manager::CalibrationConfigManager *calibration_config_manager = - base::Singleton::get(); + CalibrationConfigManager *calibration_config_manager = + Singleton::get(); - const config_manager::CameraCalibrationPtr camera_calibration = + const CameraCalibrationPtr camera_calibration = calibration_config_manager->get_camera_calibration(); - _trans_mat = camera_calibration->get_camera2car_homography_mat().cast(); -#if _DEBUG - AINFO << "homography matirx: "; - for (int i = 0; i < 3; ++i) { - AINFO << " " << _trans_mat(i, 0) << ", " - << _trans_mat(i, 1) << ", " << _trans_mat(i, 2) << "\n"; - } -#endif + trans_mat_ = camera_calibration->get_camera2car_homography_mat().cast(); // set ROI - _uv_xmin = static_cast(roi.x); - if (_uv_xmin < 0) { + uv_xmin_ = static_cast(roi.x); + if (uv_xmin_ < 0) { AERROR << "invalid ROI x min: " << roi.x; return false; } - _uv_ymin = static_cast(roi.y); - if (_uv_ymin < 0) { + uv_ymin_ = static_cast(roi.y); + if (uv_ymin_ < 0) { AERROR << "invalid ROI y min: " << roi.y; return false; } - _uv_xmax = static_cast(roi.x + roi.width - 1); - if (_uv_xmax < 0) { + uv_xmax_ = static_cast(roi.x + roi.width - 1); + if (uv_xmax_ < 0) { AERROR << "invalid ROI x max: " << roi.x + roi.width - 1; return false; } - _uv_ymax = static_cast(roi.y + roi.height - 1); - if (_uv_ymax < 0) { + uv_ymax_ = static_cast(roi.y + roi.height - 1); + if (uv_ymax_ < 0) { AERROR << "invalid ROI y max: " << roi.y + roi.height - 1; return false; } AINFO << "ROI in image space: " - << "x_min=" << _uv_xmin << ", " - << "x_max=" << _uv_xmax << ", " - << "y_min=" << _uv_ymin << ", " - << "y_max=" << _uv_ymax << "."; + << "x_min=" << uv_xmin_ << ", " + << "x_max=" << uv_xmax_ << ", " + << "y_min=" << uv_ymin_ << ", " + << "y_max=" << uv_ymax_ << "."; AINFO << "ROI width = " << roi.width << ", height = " << roi.height; - _uv_roi_cols = static_cast((_uv_xmax - _uv_xmin) / _uv_roi_x_step) + 1; - _uv_roi_rows = static_cast((_uv_ymax - _uv_ymin) / _uv_roi_y_step) + 1; - _uv_roi_count = _uv_roi_rows * _uv_roi_cols; - AINFO << "sampling step_x (u) = " << _uv_roi_x_step << ", " - << "sampling step_y (v) = " << _uv_roi_y_step; - AINFO << "ROI #columns = " << _uv_roi_cols << ", " - << "#rows = " << _uv_roi_rows; + uv_roi_cols_ = static_cast((uv_xmax_ - uv_xmin_) / uv_roi_x_step_) + 1; + uv_roi_rows_ = static_cast((uv_ymax_ - uv_ymin_) / uv_roi_y_step_) + 1; + uv_roi_count_ = uv_roi_rows_ * uv_roi_cols_; + AINFO << "sampling step_x (u) = " << uv_roi_x_step_ << ", " + << "sampling step_y (v) = " << uv_roi_y_step_; + AINFO << "ROI #columns = " << uv_roi_cols_ << ", " + << "#rows = " << uv_roi_rows_; // do point-wise transform of ROI from image space to ground space - _valid_xy.assign(_uv_roi_count, true); - _uv_2_xy.resize(_uv_roi_count); + valid_xy_.assign(uv_roi_count_, true); + uv_2_xy_.resize(uv_roi_count_); int count_fail_points = 0; int count_too_far_points = 0; - T v = _uv_ymin; - for (int i = 0; i < _uv_roi_rows; ++i) { - T u = _uv_xmin; - for (int j = 0; j < _uv_roi_cols; ++j) { + T v = uv_ymin_; + for (int i = 0; i < uv_roi_rows_; ++i) { + T u = uv_xmin_; + for (int j = 0; j < uv_roi_cols_; ++j) { // point coordinate in image space (x, y) - int id = i * _uv_roi_cols + j; + int id = i * uv_roi_cols_ + j; // transform point from image space to ego-car ground space - if (!project(u, v, &(_uv_2_xy[id]))) { - _valid_xy[id] = false; + if (!Project(u, v, &(uv_2_xy_[id]))) { + valid_xy_[id] = false; ++count_fail_points; continue; } // ignore points which is too far away from origin - if (_uv_2_xy[id].x() > max_distance || _uv_2_xy[id].y() > max_distance) { - _valid_xy[id] = false; + if (uv_2_xy_[id].x() > max_distance || uv_2_xy_[id].y() > max_distance) { + valid_xy_[id] = false; ++count_too_far_points; continue; } // - if (_uv_2_xy[id].x() < 0) { - _valid_xy[id] = false; + if (uv_2_xy_[id].x() < 0) { + valid_xy_[id] = false; ++count_too_far_points; continue; } // update xy limits - _xy_xmin = std::min(_xy_xmin, _uv_2_xy[id].x()); - _xy_ymin = std::min(_xy_ymin, _uv_2_xy[id].y()); - _xy_xmax = std::max(_xy_xmax, _uv_2_xy[id].x()); - _xy_ymax = std::max(_xy_ymax, _uv_2_xy[id].y()); + xy_xmin_ = std::min(xy_xmin_, uv_2_xy_[id].x()); + xy_ymin_ = std::min(xy_ymin_, uv_2_xy_[id].y()); + xy_xmax_ = std::max(xy_xmax_, uv_2_xy_[id].x()); + xy_ymax_ = std::max(xy_ymax_, uv_2_xy_[id].y()); - u += _uv_roi_x_step; + u += uv_roi_x_step_; } - v += _uv_roi_y_step; + v += uv_roi_y_step_; } AINFO << "#failed points = " << count_fail_points << " (" - << _uv_roi_count << ")."; + << uv_roi_count_ << ")."; AINFO << "#too far points = " << count_too_far_points << " (" - << _uv_roi_count << ")."; + << uv_roi_count_ << ")."; AINFO << " xy limits: " - << "x_min=" << _xy_xmin << ", " - << "x_max=" << _xy_xmax << ", " - << "y_min=" << _xy_ymin << ", " - << "y_max=" << _xy_ymax; + << "x_min=" << xy_xmin_ << ", " + << "x_max=" << xy_xmax_ << ", " + << "y_min=" << xy_ymin_ << ", " + << "y_max=" << xy_ymax_; - _is_vis = visualize; - if (_is_vis) { + is_vis_ = visualize; + if (is_vis_) { // build a 2D grid in ground plane and // find the index mapping for each valid point of image space - _xy_image_xmin = std::min(std::floor(_xy_xmin), static_cast(0)); - _xy_image_xmin = std::max(_xy_image_xmin, -max_distance); - _xy_image_xmax = std::max(std::ceil(_xy_xmax), static_cast(0)); - _xy_image_xmax = std::min(_xy_image_xmax, max_distance); - _xy_image_ymin = std::min(std::floor(_xy_ymin), static_cast(0)); - _xy_image_ymin = std::max(_xy_image_ymin, -max_distance); - _xy_image_ymax = std::max(std::ceil(_xy_ymax), static_cast(0)); - _xy_image_ymax = std::min(_xy_image_ymax, max_distance); - - _x_step = max_distance / static_cast(1000); - _y_step = max_distance / static_cast(1000); - - _xy_image_cols = static_cast(std::ceil( - (_xy_image_xmax - _xy_image_xmin) / _x_step)) + - 1; - _xy_image_rows = static_cast(std::ceil( - (_xy_image_ymax - _xy_image_ymin) / _y_step)) + - 1; - AINFO << "xy_image_xmin = " << _xy_image_xmin - << ", xy_image_xmax = " << _xy_image_xmax - << ", xy_image_ymin = " << _xy_image_ymin - << ", xy_image_ymax = " << _xy_image_ymax; - AINFO << "xy_image: step_x=" << _x_step << ", " - << "step_y=" << _y_step; - AINFO << "xy_image: #cols = " << _xy_image_cols - << ", #rows = " << _xy_image_rows; - - _xy_grid.resize(_xy_image_rows * _xy_image_cols); - T y = _xy_image_ymax; - for (int i = 0; i < _xy_image_rows; i++) { - T x = _xy_image_xmin; - for (int j = 0; j < _xy_image_cols; j++) { - _xy_grid[i * _xy_image_cols + j].x() = x; - _xy_grid[i * _xy_image_cols + j].y() = y; - x += _x_step; + xy_image_xmin_ = std::min(std::floor(xy_xmin_), static_cast(0)); + xy_image_xmin_ = std::max(xy_image_xmin_, -max_distance); + xy_image_xmax_ = std::max(std::ceil(xy_xmax_), static_cast(0)); + xy_image_xmax_ = std::min(xy_image_xmax_, max_distance); + xy_image_ymin_ = std::min(std::floor(xy_ymin_), static_cast(0)); + xy_image_ymin_ = std::max(xy_image_ymin_, -max_distance); + xy_image_ymax_ = std::max(std::ceil(xy_ymax_), static_cast(0)); + xy_image_ymax_ = std::min(xy_image_ymax_, max_distance); + + x_step_ = max_distance / static_cast(1000); + y_step_ = max_distance / static_cast(1000); + + xy_image_cols_ = static_cast(std::ceil( + (xy_image_xmax_ - xy_image_xmin_) / x_step_)) + 1; + xy_image_rows_ = static_cast(std::ceil( + (xy_image_ymax_ - xy_image_ymin_) / y_step_)) + 1; + AINFO << "xy_image_xmin = " << xy_image_xmin_ + << ", xy_image_xmax = " << xy_image_xmax_ + << ", xy_image_ymin = " << xy_image_ymin_ + << ", xy_image_ymax = " << xy_image_ymax_; + AINFO << "xy_image: step_x=" << x_step_ << ", " + << "step_y=" << y_step_; + AINFO << "xy_image: #cols = " << xy_image_cols_ + << ", #rows = " << xy_image_rows_; + + xy_grid_.resize(xy_image_rows_ * xy_image_cols_); + T y = xy_image_ymax_; + for (int i = 0; i < xy_image_rows_; i++) { + T x = xy_image_xmin_; + for (int j = 0; j < xy_image_cols_; j++) { + xy_grid_[i * xy_image_cols_ + j].x() = x; + xy_grid_[i * xy_image_cols_ + j].y() = y; + x += x_step_; } - y = y - _y_step; + y = y - y_step_; } - _uv_2_xy_image.resize(_uv_roi_count); - T v = _uv_ymin; - for (int i = 0; i < _uv_roi_rows; ++i) { - T u = _uv_xmin; - for (int j = 0; j < _uv_roi_cols; ++j) { - int id = i * _uv_roi_cols + j; - if (_valid_xy[id]) { - xy_2_xy_image_point(static_cast(_uv_2_xy[id].x()), - static_cast(_uv_2_xy[id].y()), - &_uv_2_xy_image[id]); + uv_2_xy_image_.resize(uv_roi_count_); + T v = uv_ymin_; + for (int i = 0; i < uv_roi_rows_; ++i) { + T u = uv_xmin_; + for (int j = 0; j < uv_roi_cols_; ++j) { + int id = i * uv_roi_cols_ + j; + if (valid_xy_[id]) { + XyToXyImagePoint(static_cast(uv_2_xy_[id].x()), + static_cast(uv_2_xy_[id].y()), + &uv_2_xy_image_[id]); } else { - _uv_2_xy_image[id].x = -1; - _uv_2_xy_image[id].y = -1; + uv_2_xy_image_[id].x = -1; + uv_2_xy_image_[id].y = -1; } - u += _uv_roi_x_step; + u += uv_roi_x_step_; } - v += _uv_roi_y_step; + v += uv_roi_y_step_; } } - _is_init = true; + is_init_ = true; AINFO << "succ. to initialize projector."; return true; } template -bool Projector::xy_2_xy_image_point(const T &x, const T &y, - cv::Point *p) const { - if (!is_valid_xy_in_xy_image(x, y)) { +bool Projector::XyToXyImagePoint(const T &x, const T &y, + cv::Point *p) const { + if (!IsValidXyInXyImage(x, y)) { return false; } - int j = static_cast(std::round((x - _xy_image_xmin) / _x_step)); + int j = static_cast(std::round((x - xy_image_xmin_) / x_step_)); if (j < 0) { return false; } - if (j >= _xy_image_cols) { + if (j >= xy_image_cols_) { return false; } - int i = static_cast(std::round((y - _xy_image_ymin) / _y_step)); + int i = static_cast(std::round((y - xy_image_ymin_) / y_step_)); if (i < 0) { return false; } - if (i >= _xy_image_rows) { + if (i >= xy_image_rows_) { return false; } - i = (_xy_image_rows - 1) - i; + i = (xy_image_rows_ - 1) - i; p->x = j; p->y = i; @@ -398,104 +365,105 @@ bool Projector::xy_2_xy_image_point(const T &x, const T &y, } template -bool Projector::is_valid_uv(const T &x, const T &y) const { - if (!(x >= _uv_xmin && x <= _uv_xmax && y >= _uv_ymin && y <= _uv_ymax)) { +bool Projector::IsValidUv(const T &x, const T &y) const { + if (!(x >= uv_xmin_ && x <= uv_xmax_ && y >= uv_ymin_ && y <= uv_ymax_)) { AINFO << "image point " << "(" << x << ", " << y << ")" << " is not in ROI: " - << "x_min=" << _uv_xmin << ", " - << "x_max=" << _uv_xmax << ", " - << "y_min=" << _uv_ymin << ", " - << "y_max=" << _uv_ymax << ". "; + << "x_min=" << uv_xmin_ << ", " + << "x_max=" << uv_xmax_ << ", " + << "y_min=" << uv_ymin_ << ", " + << "y_max=" << uv_ymax_ << ". "; return false; } return true; } template -bool Projector::uv_2_xy(const T &u, const T &v, - Eigen::Matrix *p) const { +bool Projector::UvToXy(const T &u, const T &v, + Eigen::Matrix *p) const { if (p == nullptr) { AERROR << "point pointer is null."; return false; } - if (!_is_init) { + if (!is_init_) { AERROR << "projector is not initialized."; return false; } - if (!is_valid_uv(u, v)) { + if (!IsValidUv(u, v)) { return false; } - int id = static_cast(std::round((v - _uv_ymin) / _uv_roi_y_step)) * - _uv_roi_cols + - static_cast(std::round((u - _uv_xmin) / _uv_roi_x_step)); - if (id < 0 || id >= _uv_roi_count) { + int id = static_cast(std::round((v - uv_ymin_) / uv_roi_y_step_)) * + uv_roi_cols_ + + static_cast(std::round((u - uv_xmin_) / uv_roi_x_step_)); + if (id < 0 || id >= uv_roi_count_) { AERROR << "pixel id is not valid: " << id; return false; } - if (!_valid_xy.at(id)) { + if (!valid_xy_.at(id)) { AINFO << "image point " << "(" << u << ", " << v << ")" << " is not valid for transformation."; return false; } - p->x() = _uv_2_xy.at(id).x(); - p->y() = _uv_2_xy.at(id).y(); + p->x() = uv_2_xy_.at(id).x(); + p->y() = uv_2_xy_.at(id).y(); return true; } template -bool Projector::uv_2_xy_image_point(const T &u, const T &v, - cv::Point *p) const { +bool Projector::UvToXyImagePoint(const T &u, const T &v, + cv::Point *p) const { if (p == nullptr) { AERROR << "point pointer is null."; return false; } - if (!_is_init) { + if (!is_init_) { AERROR << "projector is not initialized."; return false; } - if (!is_valid_uv(u, v)) { + if (!IsValidUv(u, v)) { AINFO << "image point " << "(" << u << ", " << v << ")" << " is not in ROI."; return false; } - int id = static_cast(std::round((v - _uv_ymin) / _uv_roi_y_step)) * - _uv_roi_cols + - static_cast(std::round((u - _uv_xmin) / _uv_roi_x_step)); - if (id < 0 || id >= _uv_roi_count) { + int id = static_cast(std::round((v - uv_ymin_) / uv_roi_y_step_)) * + uv_roi_cols_ + + static_cast(std::round((u - uv_xmin_) / uv_roi_x_step_)); + if (id < 0 || id >= uv_roi_count_) { AERROR << "pixel id is not valid: " << id; return false; } - if (!_valid_xy.at(id)) { + if (!valid_xy_.at(id)) { AINFO << "image point " << "(" << u << ", " << v << ")" << " is not valid for transformation."; return false; } - p->x = _uv_2_xy_image.at(id).x; - p->y = _uv_2_xy_image.at(id).y; + p->x = uv_2_xy_image_.at(id).x; + p->y = uv_2_xy_image_.at(id).y; return true; } template -bool Projector::project(T u, T v, Eigen::Matrix *xy_point) { +bool Projector::Project(const T &u, const T &v, + Eigen::Matrix *xy_point) { if (xy_point == nullptr) { AERROR << "xy_point is a null pointer."; return false; } Eigen::Matrix uv_point(u, v, static_cast(1)); - Eigen::Matrix xy_p = _trans_mat * uv_point; + Eigen::Matrix xy_p = trans_mat_ * uv_point; T scale = xy_p(2); if (std::abs(scale) < 1e-6) { @@ -516,7 +484,7 @@ bool Projector::project(T u, T v, Eigen::Matrix *xy_point) { return true; } -} // perception -} // apollo +} // namespace perception +} // namespace apollo -#endif // MODULES_PERCEPTION_OBSTACLE_CAMERA_LANE_POST_PROCESS_COMMON_PROJECTOR_H_ +#endif // MODULES_PERCEPTION_OBSTACLE_CAMERA_LANE_POST_PROCESS_PROJECTOR_H_ diff --git a/modules/perception/obstacle/camera/lane_post_process/common/type.h b/modules/perception/obstacle/camera/lane_post_process/common/type.h index 0f2207935c..c2b41ea9f2 100644 --- a/modules/perception/obstacle/camera/lane_post_process/common/type.h +++ b/modules/perception/obstacle/camera/lane_post_process/common/type.h @@ -89,22 +89,17 @@ const int kDelayTime(0); enum MarkerShapeType { POINT = 0, LINE_SEGMENT, - POLYNOMIAL, }; enum SpaceType { IMAGE = 0, - EGO_CAR, - IPM, VEHICLE, }; typedef Eigen::Matrix Vector2D; enum AssociationMethod { - GREEDY_SEARCH = 0, - GREEDY_SLIDING_WINDOW, - GREEDY_GROUP_CONNECT, + GREEDY_GROUP_CONNECT = 0, }; struct AssociationParam { @@ -121,7 +116,7 @@ struct AssociationParam { ScalarType min_orientation_estimation_size; AssociationParam() - : method(AssociationMethod::GREEDY_SEARCH), + : method(AssociationMethod::GREEDY_GROUP_CONNECT), min_distance(0.0), max_distance(100.0), distance_weight(0.4), @@ -228,125 +223,19 @@ struct LaneInstance { bounds = box; } - static bool compare_siz(const LaneInstance &a, const LaneInstance &b) { + static bool CompareSiz(const LaneInstance &a, const LaneInstance &b) { return a.siz > b.siz; } - static bool compare_bound(const LaneInstance &a, const LaneInstance &b) { + static bool CompareBound(const LaneInstance &a, const LaneInstance &b) { return a.bounds(0) < b.bounds(0); // x_min } - bool has_overlap(const LaneInstance &a) { + bool HasOverlap(const LaneInstance &a) { return a.bounds(0) <= this->bounds(2) && a.bounds(2) >= this->bounds(0); } }; -struct CCLanePoint { - Vector2D pos; - Vector2D orie; - ScalarType angle; - Vector2D image_pos; - ScalarType confidence; - int frame_id; - ScalarType score; - int point_id; - Eigen::Matrix power_x; - - CCLanePoint() - : pos(0.0, 0.0), - orie(1.0, 0.0), - angle(0.0), - image_pos(0.0, 0.0), - confidence(1.0), - frame_id(-1), - score(0.0), - point_id(-1) { - power_x.setZero(); - } - - static bool compare_score(const CCLanePoint &a, const CCLanePoint &b) { - return a.score > b.score; - } -}; - -struct CCLaneSegment { - ScalarType start; - ScalarType end; - std::vector> points; - PolyModel model; - int order; - - CCLaneSegment() - : start(std::numeric_limits::max()), - end(-std::numeric_limits::max()) { - model.setZero(); - order = 0; - } - - void add_point(const std::shared_ptr &p) { - points.push_back(p); - start = std::min(start, p->pos(0)); - end = std::max(end, p->pos(0)); - } -}; - -struct CCLaneTrack { - SpaceType space_type; - std::vector segments; - size_t tot_point_num; - Eigen::Matrix y_values; - ScalarType lateral_dist; - Bbox bbox; - - CCLaneTrack() : space_type(SpaceType::EGO_CAR) { - tot_point_num = 0; - segments.clear(); - lateral_dist = 0.0; - bbox << std::numeric_limits::max(), // x_min - std::numeric_limits::max(), // y_min - -std::numeric_limits::max(), // x_max - -std::numeric_limits::max(); // y_max - } - - Bbox compute_bound() { - bbox(0) = std::numeric_limits::max(); // x_min - bbox(1) = std::numeric_limits::max(); // y_min - bbox(2) = -std::numeric_limits::max(); // x_max - bbox(3) = -std::numeric_limits::max(); // y_max - - for (auto it_segment = segments.begin(); it_segment != segments.end(); - ++it_segment) { - for (size_t i = 0; i < it_segment->points.size(); ++i) { - bbox(0) = std::min(it_segment->points[i]->pos(0), bbox(0)); - bbox(1) = std::min(it_segment->points[i]->pos(1), bbox(1)); - bbox(2) = std::max(it_segment->points[i]->pos(0), bbox(2)); - bbox(3) = std::max(it_segment->points[i]->pos(1), bbox(3)); - } - } - return bbox; - } - - ScalarType size() const { - return std::max(bbox(2) - bbox(0), bbox(3) - bbox(1)); - } - - ScalarType x_min() const { - return bbox(0); - } - ScalarType y_min() const { - return bbox(1); - } - ScalarType x_max() const { - return bbox(2); - } - ScalarType y_max() const { - return bbox(3); - } -}; - -typedef std::shared_ptr CCLaneTrackPtr; -typedef const std::shared_ptr CCLaneTrackConstPtr; - struct L3CubicCurve { float x_start; float x_end; @@ -403,7 +292,7 @@ struct LaneObject { confidence.reserve(100); } - std::string get_spatial_label() const { + std::string GetSpatialLabel() const { switch (spatial) { case SpatialLabelType::L_0: return "L0"; @@ -471,7 +360,7 @@ struct LaneObject { } */ - void copy_to(LaneObject* new_lane_object) { + void CopyTo(LaneObject* new_lane_object) { new_lane_object->point_num = point_num; new_lane_object->spatial = spatial; new_lane_object->semantic = semantic; @@ -543,6 +432,10 @@ typedef std::vector LaneObjects; typedef std::shared_ptr LaneObjectsPtr; typedef const std::shared_ptr LaneObjectsConstPtr; +typedef std::vector LaneInstances; +typedef std::shared_ptr LaneInstancesPtr; +typedef const std::shared_ptr LaneInstancesConstPtr; + // typedef adu::perception::obstacle::transformer_tool:: // Projector Projector; diff --git a/modules/perception/obstacle/camera/lane_post_process/common/util.h b/modules/perception/obstacle/camera/lane_post_process/common/util.h index 95493f8dcb..6965178fef 100644 --- a/modules/perception/obstacle/camera/lane_post_process/common/util.h +++ b/modules/perception/obstacle/camera/lane_post_process/common/util.h @@ -31,7 +31,10 @@ namespace apollo { namespace perception { // @brief: convert angle from the range of [-pi, pi] to [0, 2*pi] -inline void rect_angle(ScalarType* theta) { +inline void RectAngle(ScalarType* theta) { + if (theta == NULL) { + return; + } if (*theta < 0) { (*theta) += static_cast(2 * M_PI); } @@ -39,7 +42,7 @@ inline void rect_angle(ScalarType* theta) { // @brief: fit polynomial function with QR decomposition (using Eigen 3) template -bool poly_fit(const std::vector>& pos_vec, +bool PolyFit(const std::vector>& pos_vec, const int& order, Eigen::Matrix* coeff, const bool& is_x_axis = true) { if (coeff == NULL) { @@ -85,7 +88,7 @@ bool poly_fit(const std::vector>& pos_vec, // @brief: evaluate y value of given x for a polynomial function template -T poly_eval(const T& x, const int& order, +T PolyEval(const T& x, const int& order, const Eigen::Matrix& coeff) { int poly_order = order; if (order > MAX_POLY_ORDER) { -- GitLab