diff --git a/modules/perception/obstacle/base/BUILD b/modules/perception/obstacle/base/BUILD index 919c94520f7ca0a9a3f133b835433c76aa5c7380..aab753e1e866e8c3a3bdb7bad9cc6186adb086db 100644 --- a/modules/perception/obstacle/base/BUILD +++ b/modules/perception/obstacle/base/BUILD @@ -22,6 +22,7 @@ cc_library( "//modules/perception/lib/base", "//modules/perception/lib/pcl_util", "//modules/perception/proto:perception_proto", + "//modules/perception/obstacle/camera/lane_post_process/common:perception_obstacle_camera_lane_post_process_common_type", "@eigen", "@opencv2//:core", ], diff --git a/modules/perception/obstacle/base/object.h b/modules/perception/obstacle/base/object.h index b44e9bbd242645d85a9e83ba653134c915df99a1..6a0792363de06b06c9eb84dc979df644357667d2 100644 --- a/modules/perception/obstacle/base/object.h +++ b/modules/perception/obstacle/base/object.h @@ -30,6 +30,7 @@ #include "modules/perception/lib/pcl_util/pcl_types.h" #include "modules/perception/obstacle/base/object_supplement.h" #include "modules/perception/obstacle/base/types.h" +#include "modules/perception/obstacle/camera/lane_post_process/common/type.h" namespace apollo { namespace perception { @@ -113,6 +114,7 @@ struct SensorObjects { SeqId seq_num = 0; std::vector objects; Eigen::Matrix4d sensor2world_pose; + LaneObjectsPtr lane_objects; }; } // namespace perception diff --git a/modules/perception/obstacle/camera/lane_post_process/common/BUILD b/modules/perception/obstacle/camera/lane_post_process/common/BUILD index 968aa859b757cad3ad6caceb2d5d016729283a98..716f141a34afca10eb8ee6b317441c95e7dc8470 100644 --- a/modules/perception/obstacle/camera/lane_post_process/common/BUILD +++ b/modules/perception/obstacle/camera/lane_post_process/common/BUILD @@ -3,7 +3,7 @@ load("//tools:cpplint.bzl", "cpplint") package(default_visibility = ["//visibility:public"]) cc_library( - name = "lane_type", + name = "perception_obstacle_camera_lane_post_process_common_type", hdrs = ["type.h"], deps = [ "//modules/common:log", @@ -13,11 +13,23 @@ cc_library( ) cc_library( - name = "lane_util", + name = "perception_obstacle_camera_lane_post_process_common_util", hdrs = ["util.h"], deps = [ "//modules/common:log", - "//modules/perception/obstacle/camera/lane_post_process/common:lane_type", + "//modules/perception/obstacle/camera/lane_post_process/common:perception_obstacle_camera_lane_post_process_common_type", + ], +) + +cc_library( + name = "perception_obstacle_camera_lane_post_process_common_connected_component_analysis", + srcs = ["connected_component.cc"], + hdrs = ["connected_component.h"], + deps = [ + "//modules/common:log", + "//modules/perception/obstacle/camera/lane_post_process/common:perception_obstacle_camera_lane_post_process_common_type", + "@eigen//:eigen", + "@opencv2//:core", ], ) diff --git a/modules/perception/obstacle/camera/lane_post_process/common/connected_component.h b/modules/perception/obstacle/camera/lane_post_process/common/connected_component.h index b76dc665f78e06f5839544049487342c39a50f70..eaadd2cc20bbd8ea9f8da69b3ba642879830f158 100644 --- a/modules/perception/obstacle/camera/lane_post_process/common/connected_component.h +++ b/modules/perception/obstacle/camera/lane_post_process/common/connected_component.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,8 +14,6 @@ * limitations under the License. *****************************************************************************/ -// @brief: connected component analysis for lane detection - #ifndef MODULES_PERCEPTION_OBSTACLE_CAMERA_LANE_POST_PROCESS_COMMON_CONNECTED_COMPONENT_H_ #define MODULES_PERCEPTION_OBSTACLE_CAMERA_LANE_POST_PROCESS_COMMON_CONNECTED_COMPONENT_H_ @@ -27,8 +25,8 @@ #include #include -//#include -//#include +// #include +// #include #include "modules/common/log.h" #include "modules/perception/obstacle/camera/lane_post_process/common/type.h" @@ -346,8 +344,9 @@ class ConnectedComponentGenerator { #endif } - bool FindCC(const cv::Mat& lane_map, - std::vector>& cc); + bool FindConnectedComponents( + const cv::Mat& lane_map, + std::vector>& cc); private: #if CUDA_CC @@ -368,13 +367,13 @@ class ConnectedComponentGenerator { #if CUDA_CC int* labels_; + cudaArray* img_array_; + int* label_array_; #else DisjointSet labels_; std::vector frame_label_; #endif std::vector root_map_; - cudaArray* img_array_; - int* label_array_; }; } // namespace perception diff --git a/modules/perception/obstacle/camera/lane_post_process/common/connected_components.cc b/modules/perception/obstacle/camera/lane_post_process/common/connected_components.cc deleted file mode 100644 index c2417f4b0f7cfdfea7492c3e7386a3628ee0bae0..0000000000000000000000000000000000000000 --- a/modules/perception/obstacle/camera/lane_post_process/common/connected_components.cc +++ /dev/null @@ -1,874 +0,0 @@ -/****************************************************************************** - * Copyright 2017 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: connected component analysis for lane detection - -#include -#include -#include -#include - -#include "modules/perception/obstacle/camera/lane_post_process/common/connected_components.h" -//#include "util.cu" - -namespace apollo { -namespace perception { - -using std::shared_ptr; -using std::vector; -using std::unordered_set; -using std::atan2; -using std::numeric_limits; -using std::swap; -using std::max; -using std::min; -using std::sqrt; - -const ScalarType kEpsCross = 0.001; -const ScalarType kCloseToBboxPercentage = 0.0; -const ScalarType kCloseEdgeLength = 10.0; - -/** DisjointSet **/ -int DisjointSet::Add() { - int cur_size = static_cast(disjoint_array_.size()); - disjoint_array_.push_back(cur_size); - ++subset_num_; - return cur_size; -} - -int DisjointSet::Find(int x) { - int root = x; - while (root != disjoint_array_[root]) { - root = disjoint_array_[root]; - } - - /* - while (true) { - const int z = m_disjoint_array[x]; - if (z == x) { - break; - } - m_disjoint_array[x] = y; - x = z; - } - */ - while (disjoint_array_[x] != root) { - int temp = disjoint_array_[x]; - disjoint_array_[x] = root; - x = temp; - } - - return root; -} - -void DisjointSet::Unite(int x, int y) { - if (x == y) { - return; - } - - int x_root = Find(x); - int y_root = Find(y); - if (x_root == y_root) { - return; - } else if (x_root < y_root) { - disjoint_array_[y_root] = x_root; - } else { - disjoint_array_[x_root] = y_root; - } - - --subset_num_; -} - - -/** ConnectedComponent **/ -void ConnectedComponent::AddPixel(int x, int y) { - if (pixel_count_ == 0) { - // new bounding box - bbox_.x_min = x; // x_min - bbox_.y_min = y; // y_min - bbox_.x_max = x; // x_max - bbox_.y_max = y; // y_max - } else { - // extend bounding box if necessary - if (x < bbox_.x_min) { - bbox_.x_min = x; - } - if (x > bbox_.x_max) { - bbox_.x_max = x; - } - if (y < bbox_.y_min) { - bbox_.y_min = y; - } - if (y > bbox_.y_max) { - bbox_.y_max = y; - } - } - - pixels_->push_back(cv::Point(x, y)); - pixel_count_++; -} - -void ConnectedComponent::FindBboxPixels() { - bbox_.bbox_pixel_idx.reset(new vector); - for (int i = 0; i < pixel_count_; ++i) { - if (pixels_->at(i).x == bbox_.x_min || pixels_->at(i).x == bbox_.x_max || - pixels_->at(i).y == bbox_.y_min || pixels_->at(i).y == bbox_.y_max) { - bbox_.bbox_pixel_idx->push_back(i); - } - } -} - -ConnectedComponent::BoundingBoxSplitType ConnectedComponent::DetermineSplit( - ScalarType split_siz) { - int height = bbox_.y_max - bbox_.y_min + 1; - int width = bbox_.x_max - bbox_.x_min + 1; - ScalarType diag_len = sqrt(static_cast(height * width)); - if (diag_len >= split_siz) { - bbox_.split = (height < 5) ? BoundingBoxSplitType::HORIZONTAL - : BoundingBoxSplitType::VERTICAL; - } else { - bbox_.split = BoundingBoxSplitType::NONE; - } - return bbox_.split; -} - -void ConnectedComponent::findContourForSplit() { - if (bbox_.split == BoundingBoxSplitType::NONE) { - return; - } - - // initialize contours - if (bbox_.split == BoundingBoxSplitType::VERTICAL) { - bbox_.left_contour.reset( - new std::vector(bbox_.y_max - bbox_.y_min + 1, bbox_.x_max)); - bbox_.right_contour.reset( - new std::vector(bbox_.y_max - bbox_.y_min + 1, bbox_.x_min)); - } else if (bbox_.split == BoundingBoxSplitType::HORIZONTAL) { - bbox_.up_contour.reset( - new std::vector(bbox_.x_max - bbox_.x_min + 1, bbox_.y_max)); - bbox_.down_contour.reset( - new std::vector(bbox_.x_max - bbox_.x_min + 1, bbox_.y_min)); - } - - // find contour pixels - for (int i = 0; i < pixel_count_; ++i) { - // get contour pixels if need splitting - if (bbox_.split == BoundingBoxSplitType::VERTICAL) { - int y = pixels_->at(i).y - bbox_.y_min; - bbox_.left_contour->at(y) = - min(bbox_.left_contour->at(y), pixels_->at(i).x); - bbox_.right_contour->at(y) = - max(bbox_.right_contour->at(y), pixels_->at(i).x); - } else if (bbox_.split == BoundingBoxSplitType::HORIZONTAL) { - int x = pixels_->at(i).x - bbox_.x_min; - bbox_.up_contour->at(x) = - min(bbox_.up_contour->at(x), pixels_->at(i).y); - bbox_.down_contour->at(x) = - max(bbox_.down_contour->at(x), pixels_->at(i).y); - } - } -} - -void ConnectedComponent::findVertices() { - unordered_set left_boundary; - unordered_set up_boundary; - unordered_set right_boundary; - unordered_set down_boundary; - for (auto it = bbox_.bbox_pixel_idx->begin(); - it != bbox_.bbox_pixel_idx->end(); ++it) { - const cv::Point2i* p = &(pixels_->at(*it)); - if (p->x == x_min()) { - left_boundary.insert(p->y); - } - if (p->y == y_min()) { - up_boundary.insert(p->x); - } - if (p->x == x_max()) { - right_boundary.insert(p->y); - } - if (p->y == y_max()) { - down_boundary.insert(p->x); - } - } - - vertices_.reset(new vector); - for (auto it = bbox_.bbox_pixel_idx->begin(); - it != bbox_.bbox_pixel_idx->end(); ++it) { - const cv::Point2i* p = &(pixels_->at(*it)); - if (p->x == x_min() && p->y == y_max()) { - // bottom-left corner - if (down_boundary.find(p->x + 1) == down_boundary.end() || - left_boundary.find(p->y - 1) == left_boundary.end()) { - vertices_->push_back(Vertex(p->x, p->y)); - } - } else if (p->x == x_min() && p->y == y_min()) { - // upper-left corner - if (up_boundary.find(p->x + 1) == up_boundary.end() || - left_boundary.find(p->y + 1) == left_boundary.end()) { - vertices_->push_back(Vertex(p->x, p->y)); - } - } else if (p->x == x_max() && p->y == y_min()) { - // upper-right corner - if (up_boundary.find(p->x - 1) == up_boundary.end() || - right_boundary.find(p->y + 1) == right_boundary.end()) { - vertices_->push_back(Vertex(p->x, p->y)); - } - } else if (p->x == x_min() && p->y == y_max()) { - // bottom-right corner - if (down_boundary.find(p->x - 1) == down_boundary.end() || - right_boundary.find(p->y - 1) == right_boundary.end()) { - vertices_->push_back(Vertex(p->x, p->y)); - } - } else { - // other bounding box pixels - if (p->x == x_min()) { - // on left boundary - if (left_boundary.find(p->y - 1) == left_boundary.end() || - left_boundary.find(p->y + 1) == left_boundary.end()) { - vertices_->push_back(Vertex(p->x, p->y)); - } - } else if (p->y == y_min()) { - // on upper boundary - if (up_boundary.find(p->x - 1) == up_boundary.end() || - up_boundary.find(p->x + 1) == up_boundary.end()) { - vertices_->push_back(Vertex(p->x, p->y)); - } - } else if (p->x == x_max()) { - // on right boundary - if (right_boundary.find(p->y - 1) == right_boundary.end() || - right_boundary.find(p->y + 1) == right_boundary.end()) { - vertices_->push_back(Vertex(p->x, p->y)); - } - } else if (p->y == y_max()) { - // on lower boundary - if (down_boundary.find(p->x - 1) == down_boundary.end() || - down_boundary.find(p->x + 1) == down_boundary.end()) { - vertices_->push_back(Vertex(p->x, p->y)); - } - } else { - AERROR << "the point " << "(" << p->x << ", " << p->y << ")" - << " is not on bounding box."; - } - } - } -} - -ConnectedComponent::Edge ConnectedComponent::MakeEdge(int i, int j) { - ConnectedComponent::Edge edge; - edge.start_vertex_id = i; - edge.end_vertex_id = j; - const Vertex& start_vertex = vertices_->at(i); - const Vertex& end_vertex = vertices_->at(j); - edge.vec(0) = end_vertex(0) - start_vertex(0); - edge.vec(1) = end_vertex(1) - start_vertex(1); - edge.len = edge.vec.norm(); - edge.orie = atan2(edge.vec(1), edge.vec(0)); - return edge; -} - -void ConnectedComponent::FindEdges() { - if (GetVertexCount() <= 1) { - return; - } - - // construct candidate edges from vertices - edges_.reset(new vector); - ScalarType max_len = numeric_limits::min(); - max_len_edge_id_ = -1; - for (int i = 0; i < GetVertexCount(); ++i) { - for (int j = i + 1; j < GetVertexCount(); ++j) { - if (vertices_->at(i)(1) >= vertices_->at(j)(1)) { - edges_->push_back(MakeEdge(i, j)); - } else { - edges_->push_back(MakeEdge(j, i)); - } - if (edges_->back().len > max_len) { - max_len = edges_->back().len; - max_len_edge_id_ = static_cast(edges_->size()) - 1; - } - } - } - - // initialize clockwise and anticlockwise edges - const Edge& max_len_edge = edges_->at(max_len_edge_id_); - - clockwise_edge_->start_vertex_id = max_len_edge.start_vertex_id; - clockwise_edge_->end_vertex_id = max_len_edge.end_vertex_id; - clockwise_edge_->len = max_len_edge.len; - clockwise_edge_->orie = max_len_edge.orie; - clockwise_edge_->vec = max_len_edge.vec; - - anticlockwise_edge_->start_vertex_id = max_len_edge.start_vertex_id; - anticlockwise_edge_->end_vertex_id = max_len_edge.end_vertex_id; - anticlockwise_edge_->len = max_len_edge.len; - anticlockwise_edge_->orie = max_len_edge.orie; - anticlockwise_edge_->vec = max_len_edge.vec; - - // find the clockwise and anticlockwise edges - Vertex q; - Displacement new_vec; - ScalarType cross; - for (int i = 0; i < GetVertexCount(); ++i) { - const Vertex& p = vertices_->at(i); - - // search the clockwise edge - q = vertices_->at(clockwise_edge_->start_vertex_id); - new_vec = p - q; - cross = clockwise_edge_->vec(0) * new_vec(1) - - new_vec(0) * clockwise_edge_->vec(1); - if (cross > kEpsCross) { - if (clockwise_edge_->vec(0) >= 0) { - // from left to right - if (p(0) == static_cast(x_max()) || - p(1) == static_cast(y_min())) { - clockwise_edge_->end_vertex_id = i; - } else { - clockwise_edge_->start_vertex_id = i; - } - } else { - // from right to left - if (p(0) == static_cast(x_min()) || - p(1) == static_cast(y_min())) { - clockwise_edge_->end_vertex_id = i; - } else { - clockwise_edge_->start_vertex_id = i; - } - } - const Vertex& new_start_vertex = - vertices_->at(clockwise_edge_->start_vertex_id); - const Vertex& new_end_vertex = - vertices_->at(clockwise_edge_->end_vertex_id); - clockwise_edge_->vec(0) = new_end_vertex(0) - new_start_vertex(0); - clockwise_edge_->vec(1) = new_end_vertex(1) - new_start_vertex(1); - } - - // search the anticlockwise edge - q = vertices_->at(anticlockwise_edge_->start_vertex_id); - new_vec = p - q; - cross = anticlockwise_edge_->vec(0) * new_vec(1) - - new_vec(0) * anticlockwise_edge_->vec(1); - if (cross < -kEpsCross) { - if (anticlockwise_edge_->vec(0) <= 0) { - // from right to left - if (p(0) == static_cast(x_min()) || - p(1) == static_cast(y_min())) { - anticlockwise_edge_->end_vertex_id = i; - } else { - anticlockwise_edge_->start_vertex_id = i; - } - } else { - // from left to right - if (p(0) == static_cast(x_max()) || - p(1) == static_cast(y_min())) { - anticlockwise_edge_->end_vertex_id = i; - } else { - anticlockwise_edge_->start_vertex_id = i; - } - } - const Vertex& new_start_vertex = - vertices_->at(anticlockwise_edge_->start_vertex_id); - const Vertex& new_end_vertex = - vertices_->at(anticlockwise_edge_->end_vertex_id); - anticlockwise_edge_->vec(0) = new_end_vertex(0) - new_start_vertex(0); - anticlockwise_edge_->vec(1) = new_end_vertex(1) - new_start_vertex(1); - } - } - - clockwise_edge_->len = clockwise_edge_->vec.norm(); - clockwise_edge_->orie = - atan2(clockwise_edge_->vec(1), clockwise_edge_->vec(0)); - - anticlockwise_edge_->len = anticlockwise_edge_->vec.norm(); - anticlockwise_edge_->orie = - atan2(anticlockwise_edge_->vec(1), anticlockwise_edge_->vec(0)); - - clockwise_edges_->push_back(Edge()); - clockwise_edges_->back().start_vertex_id = clockwise_edge_->start_vertex_id; - clockwise_edges_->back().end_vertex_id = clockwise_edge_->end_vertex_id; - clockwise_edges_->back().vec = clockwise_edge_->vec; - clockwise_edges_->back().orie = clockwise_edge_->orie; - clockwise_edges_->back().len = clockwise_edge_->len; - - anticlockwise_edges_->push_back(Edge()); - anticlockwise_edges_->back().start_vertex_id = - anticlockwise_edge_->start_vertex_id; - anticlockwise_edges_->back().end_vertex_id = - anticlockwise_edge_->end_vertex_id; - anticlockwise_edges_->back().vec = anticlockwise_edge_->vec; - anticlockwise_edges_->back().orie = anticlockwise_edge_->orie; - anticlockwise_edges_->back().len = anticlockwise_edge_->len; - - if (max_len_edge.vec(0) >= 0) { - // direction from left to right - inner_edge_ = clockwise_edge_; - inner_edges_ = clockwise_edges_; - } else { - // direction from right to left - inner_edge_ = anticlockwise_edge_; - inner_edges_ = anticlockwise_edges_; - } -} - -void ConnectedComponent::SplitContourVertical(int start_vertex_id, - int end_vertex_id, - int len_split, - bool is_clockwise) { - int start_pos = - static_cast(vertices_->at(start_vertex_id)(1)); // y_start - int end_pos = static_cast(vertices_->at(end_vertex_id)(1)); // y_end - - int height = start_pos - end_pos + 1; - vector lens = get_split_ranges(height, len_split); - for (int k = 0; k < static_cast(lens.size()) - 1; ++k) { - end_pos = start_pos - lens[k] + 1; - int x = is_clockwise ? bbox_.right_contour->at(end_pos - this->y_min()) - : bbox_.left_contour->at(end_pos - this->y_min()); - vertices_->push_back(Vertex(x, end_pos)); - (is_clockwise ? clockwise_edges_ : anticlockwise_edges_) - ->push_back( - MakeEdge(start_vertex_id, static_cast(vertices_->size()) - 1)); - start_pos = end_pos - 1; - x = is_clockwise ? bbox_.right_contour->at(start_pos - this->y_min()) - : bbox_.left_contour->at(start_pos - this->y_min()); - vertices_->push_back(Vertex(x, start_pos)); - start_vertex_id = static_cast(vertices_->size()) - 1; - } - (is_clockwise ? clockwise_edges_ : anticlockwise_edges_) - ->push_back(MakeEdge(start_vertex_id, end_vertex_id)); -} - -void ConnectedComponent::SplitContourVertical(int len_split, bool is_clockwise, - int start_pos, int end_pos) { - int height = start_pos - end_pos + 1; - vector lens = GetSplitRanges(height, len_split); - - // create start and end vertice - int x = is_clockwise ? bbox_.right_contour->at(start_pos - this->y_min()) - : bbox_.left_contour->at(start_pos - this->y_min()); - vertices_->push_back(Vertex(x, start_pos)); - int start_vertex_id = static_cast(vertices_->size()) - 1; - - x = is_clockwise ? bbox_.right_contour->at(end_pos - this->y_min()) - : bbox_.left_contour->at(end_pos - this->y_min()); - vertices_->push_back(Vertex(x, end_pos)); - int end_vertex_id = static_cast(vertices_->size()) - 1; - - for (int k = 0; k < static_cast(lens.size()) - 1; ++k) { - end_pos = start_pos - lens[k] + 1; - x = is_clockwise ? bbox_.right_contour->at(end_pos - this->y_min()) - : bbox_.left_contour->at(end_pos - this->y_min()); - vertices_->push_back(Vertex(x, end_pos)); - (is_clockwise ? clockwise_edges_ : anticlockwise_edges_)->push_back( - MakeEdge(start_vertex_id, static_cast(vertices_->size()) - 1)); - start_pos = end_pos - 1; - x = is_clockwise ? bbox_.right_contour->at(start_pos - this->y_min()) - : bbox_.left_contour->at(start_pos - this->y_min()); - vertices_->push_back(Vertex(x, start_pos)); - start_vertex_id = static_cast(vertices_->size()) - 1; - } - (is_clockwise ? clockwise_edges_ : anticlockwise_edges_)->push_back( - MakeEdge(start_vertex_id, end_vertex_id)); -} - -void ConnectedComponent::SplitContourHorizontal(int start_vertex_id, - int end_vertex_id, - int len_split, - bool is_clockwise) { - int start_pos = static_cast(vertices_->at(start_vertex_id)(0)); - int end_pos = static_cast(vertices_->at(end_vertex_id)(0)); - if (start_pos <= end_pos) { - // direction from left to right - int width = end_pos - start_pos + 1; - vector lens = GetSplitRanges(width, len_split); - for (int k = 0; k < static_cast(lens.size()) - 1; ++k) { - end_pos = start_pos + lens[k] - 1; - int y = (is_clockwise ? bbox_.down_contour : bbox_.up_contour) - ->at(end_pos - this->x_min()); - vertices_->push_back(Vertex(end_pos, y)); - (is_clockwise ? clockwise_edges_ : anticlockwise_edges_) - ->push_back(MakeEdge(start_vertex_id, - static_cast(vertices_->size()) - 1)); - start_pos = end_pos + 1; - y = (is_clockwise ? bbox_.down_contour : bbox_.up_contour) - ->at(start_pos - this->x_min()); - vertices_->push_back(Vertex(start_pos, y)); - start_vertex_id = static_cast(vertices_->size()) - 1; - } - (is_clockwise ? clockwise_edges_ : anticlockwise_edges_)->push_back( - MakeEdge(start_vertex_id, end_vertex_id)); - } else { - // direction from right to left - int width = start_pos - end_pos + 1; - vector lens = GetSplitRanges(width, len_split); - for (int k = 0; k < static_cast(lens.size()) - 1; ++k) { - end_pos = start_pos - lens[k] + 1; - int y = (is_clockwise ? bbox_.up_contour : bbox_.down_contour) - ->at(end_pos - this->x_min()); - vertices_->push_back(Vertex(end_pos, y)); - (is_clockwise ? clockwise_edges_ : anticlockwise_edges_) - ->push_back(MakeEdge(start_vertex_id, - static_cast(vertices_->size()) - 1)); - start_pos = end_pos - 1; - y = (is_clockwise ? bbox_.up_contour : bbox_.down_contour) - ->at(start_pos - this->x_min()); - vertices_->push_back(Vertex(start_pos, y)); - start_vertex_id = static_cast(vertices_->size()) - 1; - } - (is_clockwise ? clockwise_edges_ : anticlockwise_edges_) - ->push_back(MakeEdge(start_vertex_id, end_vertex_id)); - } -} - -void ConnectedComponent::SplitContourHorizontal(int len_split, - bool is_clockwise, - int start_pos, - int end_pos) { - if (start_pos <= end_pos) { - // direction from left to right - int y = (is_clockwise ? bbox_.down_contour : bbox_.up_contour) - ->at(start_pos - this->x_min()); - vertices_->push_back(Vertex(start_pos, y)); - int start_vertex_id = static_cast(vertices_->size()) - 1; - - y = (is_clockwise ? bbox_.down_contour : bbox_.up_contour) - ->at(end_pos - this->x_min()); - vertices_->push_back(Vertex(end_pos, y)); - int end_vertex_id = static_cast(vertices_->size()) - 1; - - int width = end_pos - start_pos + 1; - vector lens = GetSplitRanges(width, len_split); - for (int k = 0; k < static_cast(lens.size()) - 1; ++k) { - end_pos = start_pos + lens[k] - 1; - y = (is_clockwise ? bbox_.down_contour : bbox_.up_contour) - ->at(end_pos - this->x_min()); - vertices_->push_back(Vertex(end_pos, y)); - (is_clockwise ? clockwise_edges_ : anticlockwise_edges_) - ->push_back(MakeEdge(start_vertex_id, - static_cast(vertices_->size()) - 1)); - start_pos = end_pos + 1; - y = (is_clockwise ? bbox_.down_contour : bbox_.up_contour) - ->at(start_pos - this->x_min()); - vertices_->push_back(Vertex(start_pos, y)); - start_vertex_id = static_cast(vertices_->size()) - 1; - } - (is_clockwise ? clockwise_edges_ : anticlockwise_edges_) - ->push_back(MakeEdge(start_vertex_id, end_vertex_id)); - } else { - // direction from right to left - int y = (is_clockwise ? bbox_.up_contour : bbox_.down_contour) - ->at(start_pos - this->x_min()); - vertices_->push_back(Vertex(start_pos, y)); - int start_vertex_id = static_cast(vertices_->size()) - 1; - - y = (is_clockwise ? bbox_.up_contour : bbox_.down_contour) - ->at(end_pos - this->x_min()); - vertices_->push_back(Vertex(end_pos, y)); - int end_vertex_id = static_cast(vertices_->size()) - 1; - - int width = start_pos - end_pos + 1; - vector lens = GetSplitRanges(width, len_split); - for (int k = 0; k < static_cast(lens.size()) - 1; ++k) { - end_pos = start_pos - lens[k] + 1; - y = (is_clockwise ? bbox_.up_contour : bbox_.down_contour) - ->at(end_pos - this->x_min()); - vertices_->push_back(Vertex(end_pos, y)); - (is_clockwise ? clockwise_edges_ : anticlockwise_edges_) - ->push_back(MakeEdge(start_vertex_id, - static_cast(vertices_->size()) - 1)); - start_pos = end_pos - 1; - y = (is_clockwise ? bbox_.up_contour : bbox_.down_contour) - ->at(start_pos - this->x_min()); - vertices_->push_back(Vertex(start_pos, y)); - start_vertex_id = static_cast(vertices_->size()) - 1; - } - (is_clockwise ? clockwise_edges_ : anticlockwise_edges_) - ->push_back(MakeEdge(start_vertex_id, end_vertex_id)); - } -} - -// version 2 -void ConnectedComponent::SplitContour(int split_len) { - if (bbox_.split == BoundingBoxSplitType::NONE) { - return; - } - - clockwise_edges_->clear(); - anticlockwise_edges_->clear(); - - if (bbox_.split == BoundingBoxSplitType::VERTICAL) { - // split clockwise contour - SplitContourVertical(split_len, true, y_max(), y_min()); - // split anticlockwise contour - SplitContourVertical(split_len, false, y_max(), y_min()); - - } else if (bbox_.split == BoundingBoxSplitType::HORIZONTAL) { - // split clockwise contour - if (vertices_->at(clockwise_edge_->start_vertex_id)(0) <= - vertices_->at(clockwise_edge_->end_vertex_id)(0)) { - SplitContourHorizontal(split_len, true, x_min(), x_max()); - } else { - SplitContourHorizontal(split_len, true, x_max(), x_min()); - } - // split anticlockwise contour - if (vertices_->at(anticlockwise_edge_->start_vertex_id)(0) <= - vertices_->at(anticlockwise_edge_->end_vertex_id)(0)) { - SplitContourHorizontal(split_len, false, x_min(), x_max()); - } else { - SplitContourHorizontal(split_len, false, x_max(), x_min()); - } - - } else { - AFATAL << "unknown bounding box split type: " << bbox_.split; - } -} - -void ConnectedComponent::Process(ScalarType split_siz, int split_len) { - FindBboxPixels(); - FindVertices(); - FindEdges(); - if (DetermineSplit(split_siz) != - ConnectedComponent::BoundingBoxSplitType::NONE) { - FindContourForSplit(); - SplitContour(split_len); - } -} - -/** split a CC into several smaller ones **/ -vector ConnectedComponent::GetSplitRanges(int siz, int len_split) { - if (siz <= 0) { - AFATAL << "siz should be a positive number: " << siz; - } - if (len_split <= 0) { - AFATAL << "len_split should be a positive number: " << len_split; - } - - int num_split = siz / len_split; - int remainder = siz % len_split; - vector lens(num_split, len_split); - - if (lens.size() == 0 || remainder > len_split / 2) { - lens.push_back(remainder); - ++num_split; - } else { - lens.back() += remainder; - } - - return lens; -} - - -/* connected component generator */ -ConnectedComponentGenerator::ConnectedComponentGenerator(int image_width, - int image_height) - : image_width_(image_width), - image_height_(image_height), - width_(image_width), - height_(image_height), - roi_x_min_(0), - roi_y_min_(0), - roi_x_max_(image_width - 1), - roi_y_max_(image_height - 1) { - total_pix_ = - static_cast(image_width_) * static_cast(image_height_); -#if CUDA_CC - cudaChannelFormatDesc uchar_desc = cudaCreateChannelDesc(); - cudaMallocArray(&img_array_, &uchar_desc, static_cast(width_), - static_cast(height_)); - cudaBindTextureToArray(img_tex, img_array_, uchar_desc); - cudaMalloc( - (void**)&label_array_, - static_cast(width_) * static_cast(height_) * sizeof(int)); - labels_ = static_cast(malloc(total_pix_ * sizeof(int))); -#else - labels_.Init(total_pix_); - frame_label_.resize(total_pix_, -1); -#endif - root_map_.reserve(total_pix_); -} - -ConnectedComponentGenerator::ConnectedComponentGenerator(int image_width, - int image_height, - cv::Rect roi) - : image_width_(image_width), - image_height_(image_height), - width_(roi.width), - height_(roi.height), - roi_x_min_(roi.x), - roi_y_min_(roi.y), - roi_x_max_(roi.x + roi.width - 1), - roi_y_max_(roi.y + roi.height - 1) { - if (roi_x_min_ < 0) { - AFATAL << "x_min is less than zero: " << roi_x_min_; - } - if (roi_y_min_ < 0) { - AFATAL << "y_min is less than zero: " << roi_y_min_; - } - if (roi_x_max_ >= image_width_) { - AFATAL << "x_max is larger than image width: " - << roi_x_max_ << "|" << image_width_; - } - if (roi_y_max_ >= image_height_) { - AFATAL << "y_max is larger than image height: " - << roi_y_max_ << "|" << image_height_; - } - total_pix_ = static_cast(width_) * static_cast(height_); -#if _CUDA_CC - cudaChannelFormatDesc uchar_desc = cudaCreateChannelDesc(); - img_array_ = NULL; - cudaMallocArray(&img_array_, &uchar_desc, - static_cast(width_), - static_cast(height_)); - cudaBindTextureToArray(img_tex, img_array_, uchar_desc); - - cudaMalloc( - (void**)&label_array_, - static_cast(width_) * static_cast(height_) * sizeof(int)); - - cudaError_t cuda_err = cudaGetLastError(); - if (cuda_err != cudaSuccess) { - AFATAL << "failed to initialize 'img_array' and 'label_array' with CUDA: " - << cudaGetErrorString(cuda_err); - } - - labels_ = static_cast(malloc(total_pix_ * sizeof(int))); -#else - labels_.Init(total_pix_); - frame_label_.resize(total_pix_, -1); -#endif - root_map_.reserve(total_pix_); -} - -bool ConnectedComponentGenerator::find_cc( - const cv::Mat& lane_map, vector>& cc) { - if (lane_map.empty()) { - AERROR << "input lane map is empty"; - return false; - } - if (lane_map.type() != CV_8UC1) { - AERROR << "input lane map type is not CV_8UC1"; - return false; - } - - if (lane_map.cols != image_width_) { - AERROR << "The width of input lane map does not match"; - return false; - } - if (lane_map.rows != image_height_) { - AERROR << "The height of input lane map does not match"; - return false; - } - - cc.clear(); - - labels_.reset(); - root_map_.clear(); - - int x = 0; - int y = 0; - const uchar* cur_p = NULL; - const uchar* prev_p = lane_map.ptr(roi_y_min_); - int left_val = 0; - int up_val = 0; - int cur_idx = 0; - int left_idx = 0; - int up_idx = 0; - - // first loop logic - for (y = roi_y_min_; y <= roi_y_max_; y++) { - cur_p = lane_map.ptr(y); - for (x = roi_x_min_; x <= roi_x_max_; x++, cur_idx++) { - left_idx = cur_idx - 1; - up_idx = cur_idx - image_width_; - - if (x == roi_x_min_) { - left_val = 0; - } else { - left_val = cur_p[x - 1]; - } - - if (y == roi_y_min_) { - up_val = 0; - } else { - up_val = prev_p[x]; - } - - if (cur_p[x] > 0) { - if (left_val == 0 && up_val == 0) { - // current pixel is foreground and has no connected neighbors - frame_label_[cur_idx] = labels_.add(); - root_map_.push_back(-1); - } else if (left_val != 0 && up_val == 0) { - // current pixel is foreground and has left neighbor connected - frame_label_[cur_idx] = frame_label_[left_idx]; - } else if (left_val == 0 && up_val != 0) { - // current pixel is foreground and has up neighbor connect - frame_label_[cur_idx] = frame_label_[up_idx]; - } else { - // current pixel is foreground and is connected to left and up - // neighbors - frame_label_[cur_idx] = - (frame_label_[left_idx] > frame_label_[up_idx]) - ? frame_label_[up_idx] - : frame_label_[left_idx]; - labels_.Unite(frame_label_[left_idx], frame_label_[up_idx]); - } - } else { - frame_label_[cur_idx] = -1; - } - } // end for x - prev_p = cur_p; - } // end for y - if (root_map_.size() != labels_.num()) { - AERROR << "the size of root map and labels are not equal."; - return false; - } - AINFO << "subset number = " << labels_.size(); - - // second loop logic - cur_idx = 0; - int curt_label = 0; - int cc_count = 0; - for (y = roi_y_min_; y <= roi_y_max_; y++) { - for (x = roi_x_min_; x <= roi_x_max_; x++, cur_idx++) { - curt_label = frame_label_[cur_idx]; - if (curt_label >= 0) { - if (curt_label >= static_cast(labels_.num())) { - AERROR << "curt_label should be smaller than labels.num(): " - << curt_label << " vs. " << labels_.num(); - return false; - } - curt_label = labels_.find(curt_label); - if (curt_label >= static_cast(root_map_.size())) { - AERROR << "curt_label should be smaller than root_map.size() " - << curt_label << " vs. " << root_map_.size(); - return false; - } - if (root_map_[curt_label] != -1) { - cc[root_map_[curt_label]]->addPixel(x, y); - } else { - cc.push_back(std::make_shared(x, y)); - root_map_[curt_label] = cc_count++; - } - } - } // end for x - } // end for y - AINFO << "cc number = " << cc_count; - - return true; -} - -} // namespace perception -} // namespace apollo 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 ab3b9d0b578e8afa834b6b72cfd1103d4f287d1e..0f2207935c885abfb360e3495f0e681cfda9755a 100644 --- a/modules/perception/obstacle/camera/lane_post_process/common/type.h +++ b/modules/perception/obstacle/camera/lane_post_process/common/type.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. @@ -477,7 +477,7 @@ struct LaneObject { new_lane_object->semantic = semantic; new_lane_object->is_compensated = is_compensated; - for (size_t i = 0; i < new_lane_object.point_num; ++i) { + for (size_t i = 0; i < new_lane_object->point_num; ++i) { new_lane_object->pos.push_back(pos[i]); new_lane_object->orie.push_back(orie[i]); new_lane_object->image_pos.push_back(image_pos[i]); 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 b4c68dda42feb43f1d6818871aaca9e5cf2cbee3..95493f8dcb8e4b27b2694035899279afd34fab70 100644 --- a/modules/perception/obstacle/camera/lane_post_process/common/util.h +++ b/modules/perception/obstacle/camera/lane_post_process/common/util.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. @@ -90,7 +90,7 @@ T poly_eval(const T& x, const int& order, int poly_order = order; if (order > MAX_POLY_ORDER) { AERROR << "the order of polynomial function must be smaller than " - << MAX_POLY_ORDER; + << MAX_POLY_ORDER; AINFO << "forcing polynomial order to " << MAX_POLY_ORDER; poly_order = MAX_POLY_ORDER; }