[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'

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",
     ]
 )
 

modules/perception/obstacle/camera/interface/base_lane_post_processor.h

+/******************************************************************************
+ * 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 <Eigen/Core>
+#include <opencv2/opencv.hpp>
+
+#include <string>
+#include <vector>
+
+#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_

modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/BUILD

+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()

modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor.h

+/******************************************************************************
+ * 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 <Eigen/Core>
+#include <opencv2/opencv.hpp>
+
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#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<LaneFrame>& cur_frame() { return cur_frame_; }
+
+  std::shared_ptr<std::vector<LaneInstance>>& 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<ConnectedComponentGenerator> cc_generator_;
+  std::shared_ptr<LaneFrame> 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<ScalarType>> 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_

modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/lane_frame.h

+/******************************************************************************
+ * 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 <Eigen/Core>
+#include <opencv2/opencv.hpp>
+
+#include <cmath>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <unordered_set>
+#include <utility>
+#include <vector>
+
+#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<ConnectedComponentPtr>& input_cc,
+            const LaneFrameOptions& options);
+
+  bool Init(const std::vector<ConnectedComponentPtr>& input_cc,
+            const std::shared_ptr<Projector<ScalarType>>& projector,
+            const LaneFrameOptions& options);
+
+  void SetTransformer(
+      const std::shared_ptr<Projector<ScalarType>>& projector) {
+    projector_ = projector;
+    is_projector_init_ = true;
+  }
+
+  bool Process(LaneInstancesPtr instances);
+
+  int MarkerNum() const {
+    return static_cast<int>(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<int>(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<int> ComputeMarkerEdges(
+      std::vector<std::unordered_map<int, ScalarType>>* edges);
+
+  bool GreedyGroupConnectAssociation();
+
+  int AddGroupIntoGraph(const Group& group, Graph* graph,
+                        std::unordered_set<int>* 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<int>* hash_marker_idx);
+
+  void ComputeBbox();
+
+ private:
+  // options
+  LaneFrameOptions opts_;
+
+  // markers
+  std::vector<Marker> markers_;
+  int max_cc_num_;
+
+  // CC index for each marker
+  std::vector<int> cc_idx_;
+  // marker indices for each CC
+  std::unordered_map<int, std::vector<int>> cc_marker_lut_;
+
+  std::shared_ptr<const Projector<ScalarType>> projector_;
+  bool is_projector_init_;
+
+  // lane marker clusters
+  std::vector<Graph> graphs_;
+  // tight bounding boxes of lane clusters
+  std::vector<Bbox> boxes_;
+};
+
+}  // namespace perception
+}  // namespace apollo
+
+#endif  // MODULES_PERCEPTION_OBSTACLE_CAMERA_CC_LANE_POST_PROCESSOR_FRAME_H_

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()

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 <algorithm>
 #include <cmath>
 #include <limits>

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<ScalarType, 2, 1> 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<ScalarType, 1, MAX_POLY_ORDER + 1> 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<std::shared_ptr<CCLanePoint>> points;
-  PolyModel model;
-  int order;
-
-  CCLaneSegment()
-      : start(std::numeric_limits<ScalarType>::max()),
-        end(-std::numeric_limits<ScalarType>::max()) {
-    model.setZero();
-    order = 0;
-  }
-
-  void add_point(const std::shared_ptr<CCLanePoint> &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<CCLaneSegment> segments;
-  size_t tot_point_num;
-  Eigen::Matrix<ScalarType, Eigen::Dynamic, 1> 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<ScalarType>::max(),  // x_min
-        std::numeric_limits<ScalarType>::max(),      // y_min
-        -std::numeric_limits<ScalarType>::max(),     // x_max
-        -std::numeric_limits<ScalarType>::max();     // y_max
-  }
-
-  Bbox compute_bound() {
-    bbox(0) = std::numeric_limits<ScalarType>::max();   // x_min
-    bbox(1) = std::numeric_limits<ScalarType>::max();   // y_min
-    bbox(2) = -std::numeric_limits<ScalarType>::max();  // x_max
-    bbox(3) = -std::numeric_limits<ScalarType>::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<CCLaneTrack> CCLaneTrackPtr;
-typedef const std::shared_ptr<CCLaneTrack> 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<LaneObject> LaneObjects;
 typedef std::shared_ptr<LaneObjects> LaneObjectsPtr;
 typedef const std::shared_ptr<LaneObjects> LaneObjectsConstPtr;
 
+typedef std::vector<LaneInstance> LaneInstances;
+typedef std::shared_ptr<LaneInstances> LaneInstancesPtr;
+typedef const std::shared_ptr<LaneInstances> LaneInstancesConstPtr;
+
 // typedef adu::perception::obstacle::transformer_tool::
 //         Projector<ScalarType> Projector;
 

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<ScalarType>(2 * M_PI);
   }
@@ -39,7 +42,7 @@ inline void rect_angle(ScalarType* theta) {
 
 // @brief: fit polynomial function with QR decomposition (using Eigen 3)
 template <typename T = ScalarType>
-bool poly_fit(const std::vector<Eigen::Matrix<T, 2, 1>>& pos_vec,
+bool PolyFit(const std::vector<Eigen::Matrix<T, 2, 1>>& pos_vec,
               const int& order, Eigen::Matrix<T, MAX_POLY_ORDER + 1, 1>* coeff,
               const bool& is_x_axis = true) {
   if (coeff == NULL) {
@@ -85,7 +88,7 @@ bool poly_fit(const std::vector<Eigen::Matrix<T, 2, 1>>& pos_vec,
 
 // @brief: evaluate y value of given x for a polynomial function
 template <typename T = ScalarType>
-T poly_eval(const T& x, const int& order,
+T PolyEval(const T& x, const int& order,
             const Eigen::Matrix<T, MAX_POLY_ORDER + 1, 1>& coeff) {
   int poly_order = order;
   if (order > MAX_POLY_ORDER) {