Apollo2.0自动驾驶之apollo/modules/perception/obstacle/camera/cipv/cipv.h

/****************Apollo源码分析****************************

Copyright 2018 The File Authors & zouyu. All Rights Reserved.

Contact with: 1746430162@qq.com 181663309504

源码主要是c++实现的，也有少量python，git下载几百兆，其实代码不太多，主要是地图和数据了大量空间，主要程序
在apollo/modules目录中，

我们把它分成以下几部分（具体说明见各目录下的modules）：

感知：感知当前位置，速度，障碍物等等

Apollo/modules/perception

预测：对场景下一步的变化做出预测

Apollo/modules/prediction

规划：

(1) 全局路径规划：通过起点终点计算行驶路径

Apollo/modules/routing

(2) 规划当前轨道：通过感知，预测，路径规划等信息计算轨道

Apollo/modules/planning

(3) 规划转换成命令：将轨道转换成控制汽车的命令（加速，制动，转向等）

Apollo/modules/control

其它

(1) 输入输出

i. Apollo/modules/drivers 设备驱动

ii. Apollo/modules/localization 位置信息

iii. Apollo/modules/monitor 监控模块

iv. Apollo/modules/canbus 与汽车硬件交互

v. Apollo/modules/map 地图数据

vi. Apollo/modules/third_party_perception 三方感知器支持

(2) 交互

i. Apollo/modules/dreamview 可视化模块

ii. Apollo/modules/hmi 把汽车当前状态显示给用户

(3) 工具

i. Apollo/modules/calibration 标注工具

ii. Apollo/modules/common 支持其它模块的公共工具

iii. Apollo/modules/data 数据工具

iv. Apollo/modules/tools 一些Python工具

(4) 其它

i. Apollo/modules/elo 高精度定位系统，无源码，但有文档

ii. Apollo/modules/e2e 收集传感器数据给PX2,ROS

  自动驾驶系统先通过起点终点规划出整体路径（routing）；然后在行驶过程中感知（perception）当前环境
（识别车辆行人路况标志等），并预测下一步发展；然后把已知信息都传入规划模块（planning），规划出之后的轨道；
控制模块（control）将轨道数据转换成对车辆的控制信号，通过汽车交互模块（canbus）控制汽车．

我觉得这里面算法技术含量最高的是感知perception和规划planning，具体请见本博客中各模块的分析代码。

/****************************************************************************************

/******************************************************************************
 * 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.
 *****************************************************************************/

#ifndef MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H_
#define MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H_

#include <array>
#include <memory>
#include <string>

#include "Eigen/Dense"
#include "Eigen/Eigen"
#include "Eigen/Geometry"

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/perception/obstacle/base/object.h"
#include "modules/perception/obstacle/camera/common/lane_object.h"
#include "modules/perception/obstacle/camera/lane_post_process/common/type.h"

namespace apollo {
namespace perception {

struct CipvOptions {
  float velocity = 0.0f;
  float yaw_rate = 0.0f;
  float yaw_angle = 0.0f;
};

const float MAX_DIST_OBJECT_TO_LANE_METER = 20.0f;
const float MAX_VEHICLE_WIDTH_METER = 5.0f;

// TODO(All) averatge image frame rate should come from other header file.
const float AVERAGE_FRATE_RATE = 0.1f;

class Cipv {
  // Member functions
 public:
  //    friend class ::adu::perception::OnlineCalibrationService;
  Cipv(void);
  virtual ~Cipv(void);

  virtual bool Init();
  virtual std::string Name() const;

  // Determine CIPV among multiple objects
  bool DetermineCipv(std::shared_ptr<SensorObjects> sensor_objects,
                     CipvOptions *options);  // override;

 private:
  // Distance from a point to a line segment
  bool DistanceFromPointToLineSegment(const Point2Df &point,
                                      const Point2Df &line_seg_start_point,
                                      const Point2Df &line_seg_end_point,
                                      float *distance);

  // Determine CIPV among multiple objects
  bool GetEgoLane(const LaneObjectsPtr lane_objects, EgoLane *egolane_image,
                  EgoLane *egolane_ground, bool *b_left_valid,
                  bool *b_right_valid);

  // Elongate lane line
  bool ElongateEgoLane(const LaneObjectsPtr lane_objects,
                       const bool b_left_valid, const bool b_right_valid,
                       const float yaw_rate, const float velocity,
                       EgoLane *egolane_image, EgoLane *egolane_ground);

  // Get closest edge of an object in image cooridnate
  bool FindClosestEdgeOfObjectImage(const ObjectPtr &object,
                                    const EgoLane &egolane_image,
                                    LineSegment2Df *closted_object_edge);

  // Get closest edge of an object in ground cooridnate
  bool FindClosestEdgeOfObjectGround(const ObjectPtr &object,
                                     const EgoLane &egolane_ground,
                                     LineSegment2Df *closted_object_edge);

  // Check if the distance between lane and object are OK
  bool AreDistancesSane(const float distance_start_point_to_right_lane,
                        const float distance_start_point_to_left_lane,
                        const float distance_end_point_to_right_lane,
                        const float distance_end_point_to_left_lane);

  // Check if the object is in the lane in image space
  bool IsObjectInTheLaneImage(const ObjectPtr &object,
                              const EgoLane &egolane_image);
  // Check if the object is in the lane in ego-ground space
  //  |           |
  //  | *------*  |
  //  |         *-+-----*
  //  |           |  *--------* <- closest edge of object
  // *+------*    |
  //  |           |
  // l_lane     r_lane
  bool IsObjectInTheLaneGround(const ObjectPtr &object,
                               const EgoLane &egolane_ground);

  // Check if the object is in the lane in ego-ground space
  bool IsObjectInTheLane(const ObjectPtr &object, const EgoLane &egolane_image,
                         const EgoLane &egolane_ground);

  // Check if a point is left of a line segment
  bool IsPointLeftOfLine(const Point2Df &point,
                         const Point2Df &line_seg_start_point,
                         const Point2Df &line_seg_end_point);

  // Make a virtual lane line using a reference lane line and its offset
  // distance
  bool MakeVirtualLane(const LaneLine &ref_lane_line, const float yaw_rate,
                       const float offset_distance,
                       LaneLine *virtual_lane_line);

  float VehicleDynamics(const uint32_t tick, const float yaw_rate,
                        const float velocity, const float time_unit, float *x,
                        float *y);
  // Make a virtual lane line using a yaw_rate
  bool MakeVirtualEgoLaneFromYawRate(const float yaw_rate, const float velocity,
                                     const float offset_distance,
                                     LaneLine *left_lane_line,
                                     LaneLine *right_lane_line);
  // Member variables
  bool b_image_based_cipv_ = false;
  int32_t debug_level_ = 0;
  float time_unit_ = 0.0f;
  common::VehicleParam vehicle_param_;

  const float EGO_CAR_WIDTH_METER;
  const float EGO_CAR_LENGTH_METER;
  const float EGO_CAR_MARGIN_METER;
  const float EGO_CAR_VIRTUAL_LANE;
  const float EGO_CAR_HALF_VIRTUAL_LANE;
};

}  // namespace perception
}  // namespace apollo

#endif  // MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_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.

*****************************************************************************/

#ifndef MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H_

#define MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H_

#include <array>

#include <memory>

#include <string>

#include "Eigen/Dense"

#include "Eigen/Eigen"

#include "Eigen/Geometry"

#include "modules/common/configs/vehicle_config_helper.h"

#include "modules/perception/obstacle/base/object.h"

#include "modules/perception/obstacle/camera/common/lane_object.h"

#include "modules/perception/obstacle/camera/lane_post_process/common/type.h"

namespace apollo {

namespace perception {

struct CipvOptions {

float velocity = 0.0f ; //速度

float yaw_rate = 0.0f ; //弧度？

float yaw_angle = 0.0f ; //角速度？

};

const float MAX_DIST_OBJECT_TO_LANE_METER = 20.0f ; //车道宽？

const float MAX_VEHICLE_WIDTH_METER = 5.0f ; //车宽？

// TODO(All) averatge image frame rate should come from other header file.

const float AVERAGE_FRATE_RATE = 0.1f ;

class Cipv {

// Member functions Cipv的成员函数

public:

// friend class ::adu::perception::OnlineCalibrationService;

Cipv ( void );

virtual ~Cipv ( void );

virtual bool Init ();

virtual std::string Name () const ;

// Determine CIPV among multiple objects

bool DetermineCipv (std::shared_ptr < SensorObjects > sensor_objects,

CipvOptions * options); // override;

private:

// Distance from a point to a line segment 点到道路边缘的距离？

bool DistanceFromPointToLineSegment ( const Point2Df & point,

const Point2Df & line_seg_start_point,

const Point2Df & line_seg_end_point,

float * distance);

// Determine CIPV among multiple objects

bool GetEgoLane ( const LaneObjectsPtr lane_objects, EgoLane * egolane_image,

EgoLane * egolane_ground, bool * b_left_valid,

bool * b_right_valid);

// Elongate lane line 边缘延长线

bool ElongateEgoLane ( const LaneObjectsPtr lane_objects,

const bool b_left_valid, const bool b_right_valid,

const float yaw_rate, const float velocity,

EgoLane * egolane_image, EgoLane * egolane_ground);

// Get closest edge of an object in image cooridnate 在图像视野中对目标获取最近的边缘

//是不是为了让目标不越界?

bool FindClosestEdgeOfObjectImage ( const ObjectPtr & object,

const EgoLane & egolane_image,

LineSegment2Df * closted_object_edge);

// Get closest edge of an object in ground cooridnate

bool FindClosestEdgeOfObjectGround ( const ObjectPtr & object,

const EgoLane & egolane_ground,

LineSegment2Df * closted_object_edge);

// Check if the distance between lane and object are OK 确保车辆与道路边沿的距离在合更年范围内

bool AreDistancesSane ( const float distance_start_point_to_right_lane,

const float distance_start_point_to_left_lane,

const float distance_end_point_to_right_lane,

const float distance_end_point_to_left_lane);

// Check if the object is in the lane in image space

bool IsObjectInTheLaneImage ( const ObjectPtr & object,

const EgoLane & egolane_image);

// Check if the object is in the lane in ego-ground space

// | |

// | *------* |

// | *-+-----*

// | | *--------* <- closest edge of object

// *+------* |

// | |

// l_lane r_lane

bool IsObjectInTheLaneGround ( const ObjectPtr & object,

const EgoLane & egolane_ground);

// Check if the object is in the lane in ego-ground space

bool IsObjectInTheLane ( const ObjectPtr & object, const EgoLane & egolane_image,

const EgoLane & egolane_ground);

// Check if a point is left of a line segment

bool IsPointLeftOfLine ( const Point2Df & point,

const Point2Df & line_seg_start_point,

const Point2Df & line_seg_end_point);

// Make a virtual lane line using a reference lane line and its offset

// distance 使用虚化的线来拟合路上真实的线，这就像你在无人车驾驶中看到的那样，有两根线一直在拟合在一起？

bool MakeVirtualLane ( const LaneLine & ref_lane_line, const float yaw_rate,

const float offset_distance,

LaneLine * virtual_lane_line);

float VehicleDynamics ( const uint32_t tick, const float yaw_rate,

const float velocity, const float time_unit, float * x,

float * y);

// Make a virtual lane line using a yaw_rate 虚化线的角度

bool MakeVirtualEgoLaneFromYawRate ( const float yaw_rate, const float velocity,

const float offset_distance,

LaneLine * left_lane_line,

LaneLine * right_lane_line);

// Member variables

bool b_image_based_cipv_ = false ;

int32_t debug_level_ = 0 ;

float time_unit_ = 0.0f ;

common::VehicleParam vehicle_param_;

const float EGO_CAR_WIDTH_METER;

const float EGO_CAR_LENGTH_METER;

const float EGO_CAR_MARGIN_METER;

const float EGO_CAR_VIRTUAL_LANE;

const float EGO_CAR_HALF_VIRTUAL_LANE;

};

} // namespace perception

} // namespace apollo

#endif // MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H_