apollo::perception::fusion 命名空间参考

类
struct	AssociationInitOptions
struct	AssociationOptions
struct	AssociationResult
class	BaseDataAssociation
class	BaseExistenceFusion
class	BaseFilter
class	BaseFusionSystem
class	BaseGatekeeper
class	BaseMotionFusion
class	BaseShapeFusion
class	BaseTracker
class	BaseTypeFusion
struct	CameraDstTypeFusionParam
class	Dst
struct	DstCommonData
class	DstExistenceFusion
struct	DstExistenceFusionOptions
class	DstManager
struct	DstMaps
class	DstTypeFusion
struct	DstTypeFusionConfig
struct	DstTypeFusionOptions
class	DummyDataAssociation
class	DummyFusionSystem
class	DummyTracker
struct	ExistenceDstMaps
struct	ExistenceFusionInitOptions
class	FusedObject
struct	FusionComponentConfig
struct	FusionFrame
struct	FusionInitOptions
struct	FusionParams
struct	GatekeeperInitOptions
class	HMTrackersObjectsAssociation
struct	HSimilarityParams
class	InformationFilter
class	KalmanFilter
class	KalmanMotionFusion
struct	LidarDstTypeFusionParam
struct	LocSimilarityParams
class	MultiSensorFusionComponent
class	PbfGatekeeper
struct	PbfGatekeeperConfig
struct	PbfGatekeeperParams
class	PbfShapeFusion
class	PbfTracker
struct	PbfTrackerConfig
class	ProbabilisticFusion
struct	ProbabilisticFusionConfig
class	ProjectionCache
class	ProjectionCacheFrame
class	ProjectionCacheObject
class	Scene
class	Sensor
class	SensorDataManager
class	SensorFrame
struct	SensorFrameHeader
class	SensorObject
struct	ToicDstMaps
class	Track
struct	TrackerInitOptions
struct	TrackerOptions
struct	TrackInitializer
class	TrackObjectDistance
struct	TrackObjectDistanceOptions
struct	TypeFusionInitOptions
struct	WSimilarityParams
struct	XSimilarityParams
struct	YSimilarityParams

类型定义
typedef std::shared_ptr< SensorFrameHeader >	SensorFrameHeaderPtr
typedef std::shared_ptr< const SensorFrameHeader >	SensorFrameHeaderConstPtr
typedef std::shared_ptr< SensorFrame >	SensorFramePtr
typedef std::shared_ptr< const SensorFrame >	SensorFrameConstPtr
typedef std::shared_ptr< Sensor >	SensorPtr
typedef std::shared_ptr< const Sensor >	SensorConstPtr
typedef std::shared_ptr< Scene >	ScenePtr
typedef std::shared_ptr< const Scene >	SceneConstPtr
typedef std::shared_ptr< SensorObject >	SensorObjectPtr
typedef std::shared_ptr< const SensorObject >	SensorObjectConstPtr
typedef std::shared_ptr< FusedObject >	FusedObjectPtr
typedef std::map< std::string, SensorObjectPtr >	SensorId2ObjectMap
typedef std::shared_ptr< Track >	TrackPtr
typedef std::shared_ptr< const Track >	TrackConstPtr
typedef base::ConcurrentObjectPool< Track, kTrackPoolSize, TrackInitializer >	TrackPool
typedef DstCommonData *	DstCommonDataPtr
typedef ProjectionCache *	ProjectionCachePtr
typedef std::pair< size_t, size_t >	TrackMeasurmentPair

函数
bool	IsLidar (const SensorObjectConstPtr &obj)
bool	IsRadar (const SensorObjectConstPtr &obj)
bool	IsCamera (const SensorObjectConstPtr &obj)
	__attribute__ ((constructor)) void FusionPoolInitialize()
void	GetObjectEightVertices (std::shared_ptr< const base::Object > obj, EigenVector< Eigen::Vector3d > *vertices)
bool	Pt3dToCamera2d (const Eigen::Vector3d &pt3d, const Eigen::Matrix4d &world2camera_pose, base::BaseCameraModelPtr camera_model, Eigen::Vector2d *pt2d)
bool	IsObjectEightVerticesAllBehindCamera (const std::shared_ptr< const base::Object > &obj, const Eigen::Matrix4d &world2camera_pose, base::BaseCameraModelPtr camera_model)
float	ObjectInCameraView (SensorObjectConstPtr sensor_object, base::BaseCameraModelPtr camera_model, const Eigen::Affine3d &camera_sensor2world_pose, double camera_ts, double camera_max_dist, bool motion_compensation, bool all_in)
void	GetObjectEightVertices (std::shared_ptr< const base::Object > obj, apollo::common::EigenVector< Eigen::Vector3d > *vertices)
template<typename VectorType >
bool	IsPtInFrustum (const VectorType &pt, double width, double height)
	Is point lies in the frustum
template<typename Type >
Type	CalculateAugmentedIOUBBox (const base::BBox2D< Type > &box1, const base::BBox2D< Type > &box2, const Type &augmented_buffer)
	Calculate augmented IOU BBox
Dst	operator+ (const Dst &lhs, const Dst &rhs)
Dst	operator* (const Dst &dst, double w)
double	ChiSquaredCdf1TableFun (double dist)
double	ChiSquaredCdf2TableFun (double dist)
template<typename T >
void	extract_vector (const std::vector< T > &vec, const std::vector< size_t > &subset_inds, std::vector< T > *sub_vec)
double	BoundedScalePositiveProbability (double p, double max_p, double min_p)
double	ScalePositiveProbability (double p, double max_p, double th_p)
double	WelshVarLossFun (double dist, double th, double scale)
double	FuseTwoProbabilities (double prob1, double prob2)
double	FuseMultipleProbabilities (const std::vector< double > &probs)
template<typename T >
T	Bound (const T &value, const T &max_value, const T &min_value)
double	ComputePtsBoxLocationSimilarity (const ProjectionCachePtr &cache, const ProjectionCacheObject *object, const base::BBox2DF &camera_bbox)
double	ComputePtsBoxShapeSimilarity (const ProjectionCachePtr &cache, const ProjectionCacheObject *object, const base::BBox2DF &camera_bbox)
double	ComputePtsBoxSimilarity (const ProjectionCachePtr &cache, const ProjectionCacheObject *object, const base::BBox2DF &camera_bbox)
double	ComputeRadarCameraXSimilarity (const double velo_ct_x, const double camera_ct_x, const double size_x, const XSimilarityParams &params)
double	ComputeRadarCameraYSimilarity (const double velo_ct_y, const double camera_ct_y, const double size_y, const YSimilarityParams &params)
	calculate the y similarity between radar and camera
double	ComputeRadarCameraHSimilarity (const SensorObjectConstPtr &radar, const SensorObjectConstPtr &camera, const double size_y, const EigenVector< Eigen::Vector2d > &radar_box2d_vertices, const HSimilarityParams &params)
	calculate the h similarity between radar and camera
double	ComputeRadarCameraWSimilarity (const SensorObjectConstPtr &radar, const double width, const double size_x, const EigenVector< Eigen::Vector2d > &radar_box2d_vertices, const WSimilarityParams &params)
	calculate the w similarity between radar and camera
double	ComputeRadarCameraLocSimilarity (const Eigen::Vector3d &radar_ct, const SensorObjectConstPtr &camera, const Eigen::Matrix4d &world2camera_pose, const LocSimilarityParams &params)
	calculate the loc similarity between radar and camera
double	ComputeRadarCameraVelocitySimilarity (const SensorObjectConstPtr &radar, const SensorObjectConstPtr &camera)
	calculate the velocity similarity between radar and camera
template<typename Type >
std::string	vector2string (const std::vector< Type > &values)
	FUSION_REGISTER_FUSIONSYSTEM (DummyFusionSystem)
	FUSION_REGISTER_FUSIONSYSTEM (ProbabilisticFusion)
	PERCEPTION_REGISTER_REGISTERER (BaseDataAssociation)
	PERCEPTION_REGISTER_REGISTERER (BaseFusionSystem)
	PERCEPTION_REGISTER_REGISTERER (BaseGatekeeper)
	CYBER_REGISTER_COMPONENT (MultiSensorFusionComponent)

变量
struct apollo::perception::fusion::SensorFrameHeader	EIGEN_ALIGN16

类型定义说明

DstCommonDataPtr

typedef DstCommonData* apollo::perception::fusion::DstCommonDataPtr

在文件 dst_evidence.h 第 46 行定义.

FusedObjectPtr

typedef std::shared_ptr<FusedObject> apollo::perception::fusion::FusedObjectPtr

在文件 sensor_object.h 第 83 行定义.

ProjectionCachePtr

typedef ProjectionCache* apollo::perception::fusion::ProjectionCachePtr

在文件 projection_cache.h 第 194 行定义.

SceneConstPtr

typedef std::shared_ptr<const Scene> apollo::perception::fusion::SceneConstPtr

在文件 scene.h 第 89 行定义.

ScenePtr

typedef std::shared_ptr<Scene> apollo::perception::fusion::ScenePtr

在文件 scene.h 第 88 行定义.

SensorConstPtr

typedef std::shared_ptr<const Sensor> apollo::perception::fusion::SensorConstPtr

在文件 base_forward_declaration.h 第 34 行定义.

SensorFrameConstPtr

typedef std::shared_ptr<const SensorFrame> apollo::perception::fusion::SensorFrameConstPtr

在文件 base_forward_declaration.h 第 30 行定义.

SensorFrameHeaderConstPtr

typedef std::shared_ptr<const SensorFrameHeader> apollo::perception::fusion::SensorFrameHeaderConstPtr

在文件 base_forward_declaration.h 第 26 行定义.

SensorFrameHeaderPtr

typedef std::shared_ptr<SensorFrameHeader> apollo::perception::fusion::SensorFrameHeaderPtr

在文件 base_forward_declaration.h 第 25 行定义.

SensorFramePtr

typedef std::shared_ptr<SensorFrame> apollo::perception::fusion::SensorFramePtr

在文件 base_forward_declaration.h 第 29 行定义.

SensorId2ObjectMap

typedef std::map<std::string, SensorObjectPtr> apollo::perception::fusion::SensorId2ObjectMap

在文件 track.h 第 30 行定义.

SensorObjectConstPtr

typedef std::shared_ptr<const SensorObject> apollo::perception::fusion::SensorObjectConstPtr

在文件 sensor_object.h 第 68 行定义.

SensorObjectPtr

typedef std::shared_ptr<SensorObject> apollo::perception::fusion::SensorObjectPtr

在文件 sensor_object.h 第 67 行定义.

SensorPtr

typedef std::shared_ptr<Sensor> apollo::perception::fusion::SensorPtr

在文件 base_forward_declaration.h 第 33 行定义.

TrackConstPtr

typedef std::shared_ptr<const Track> apollo::perception::fusion::TrackConstPtr

在文件 track.h 第 398 行定义.

TrackMeasurmentPair

typedef std::pair<size_t, size_t> apollo::perception::fusion::TrackMeasurmentPair

在文件 base_data_association.h 第 37 行定义.

TrackPool

typedef base::ConcurrentObjectPool<Track, kTrackPoolSize, TrackInitializer> apollo::perception::fusion::TrackPool

在文件 track_pool_types.h 第 35 行定义.

TrackPtr

typedef std::shared_ptr<Track> apollo::perception::fusion::TrackPtr

在文件 track.h 第 397 行定义.

函数说明

__attribute__()

apollo::perception::fusion::__attribute__

(

(constructor)

)

在文件 track_pool_types.cc 第 25 行定义.

                                                         {
  TrackPool::Instance();
  AINFO << "Initialize FusionPool";
}

Bound()

template<typename T >

T apollo::perception::fusion::Bound	(	const T &	value,
		const T &	max_value,
		const T &	min_value
	)

在文件 probabilities.h 第 29 行定义.

                                                                {
  return std::max(min_value, std::min(max_value, value));
}

BoundedScalePositiveProbability()

double apollo::perception::fusion::BoundedScalePositiveProbability	(	double	p,
		double	max_p,
		double	min_p
	)

在文件 probabilities.cc 第 28 行定义.

                                                                             {
  p = std::max(p, min_p);
  p = (p - min_p) * (max_p - min_p) / (1 - min_p) + min_p;
  return p;
}

CalculateAugmentedIOUBBox()

template<typename Type >

Type apollo::perception::fusion::CalculateAugmentedIOUBBox	(	const base::BBox2D< Type > &	box1,
		const base::BBox2D< Type > &	box2,
		const Type &	augmented_buffer
	)

Calculate augmented IOU BBox

模板参数

Type

参数

box1
box2
augmented_buffer

返回

Type

在文件 camera_util.h 第 66 行定义.

                                                             {
  base::BBox2D<Type> augmented_box1 = box1;
  augmented_box1.xmin -= augmented_buffer;
  augmented_box1.ymin -= augmented_buffer;
  augmented_box1.xmax += augmented_buffer;
  augmented_box1.ymax += augmented_buffer;
  base::BBox2D<Type> augmented_box2 = box2;
  augmented_box2.xmin -= augmented_buffer;
  augmented_box2.ymin -= augmented_buffer;
  augmented_box2.xmax += augmented_buffer;
  augmented_box2.ymax += augmented_buffer;
  Type augmented_iou =
      algorithm::CalculateIOUBBox(augmented_box1, augmented_box2);
  return augmented_iou;
}

ChiSquaredCdf1TableFun()

double apollo::perception::fusion::ChiSquaredCdf1TableFun ( double  dist )

inline

在文件 chi_squared_cdf_1_0.0500_0.999900.h 第 69 行定义.

                                                  {
  static constexpr double step = 0.050000;
  static const size_t table_size =
      sizeof(ChiSquaredCdf1TableTable) / sizeof(double);
  double dist_ind = dist / step;
  int dist_int = static_cast<int>(dist_ind);
  double w = dist_ind - dist_int;
  if (dist_ind >= table_size - 1) {
    return 1.0;
  }
  return (ChiSquaredCdf1TableTable[dist_int] * (1 - w) +
          ChiSquaredCdf1TableTable[dist_int + 1] * w);
}

ChiSquaredCdf2TableFun()

double apollo::perception::fusion::ChiSquaredCdf2TableFun ( double  dist )

inline

在文件 chi_squared_cdf_2_0.0500_0.999900.h 第 78 行定义.

                                                  {
  static constexpr double step = 0.050000;
  static const size_t table_size =
      sizeof(ChiSquaredCdf2TableTable) / sizeof(double);
  double dist_ind = dist / step;
  int dist_int = static_cast<int>(dist_ind);
  double w = dist_ind - dist_int;
  if (dist_ind >= table_size - 1) {
    return 1.0;
  }
  return (ChiSquaredCdf2TableTable[dist_int] * (1 - w) +
          ChiSquaredCdf2TableTable[dist_int + 1] * w);
}

ComputePtsBoxLocationSimilarity()

double apollo::perception::fusion::ComputePtsBoxLocationSimilarity	(	const ProjectionCachePtr &	cache,
		const ProjectionCacheObject *	object,
		const base::BBox2DF &	camera_bbox
	)

在文件 track_object_similarity.cc 第 39 行定义.

                                                                       {
  static const double min_p = 1e-6;
  static const double max_p = 1 - 1e-6;
  double x_std_dev = 0.4;
  double y_std_dev = 0.5;
  size_t check_augmented_iou_minimum_pts_num = 20;
  float augmented_buffer = 25.0f;
  if (object->Empty()) {
    ADEBUG << "cache object is empty!";
    return min_p;
  }
  Eigen::Vector2d mean_pixel_dist(0.0, 0.0);
  // calculate mean x y pixel distance
  const size_t start_ind = object->GetStartInd();
  const size_t end_ind = object->GetEndInd();
  if (end_ind - start_ind >= check_augmented_iou_minimum_pts_num) {
    base::BBox2DF velo_bbox = object->GetBox();
    float augmented_iou =
        CalculateAugmentedIOUBBox(velo_bbox, camera_bbox, augmented_buffer);
    if (augmented_iou < kFloatEpsilon) {
      ADEBUG << "augmented iou is empty!";
      return min_p;
    }
  }
  for (size_t i = start_ind; i < end_ind; ++i) {
    auto* velo_pt2d = cache->GetPoint2d(i);
    if (velo_pt2d == nullptr) {
      AERROR << "query pt from projection cache failed!";
      continue;
    }
    if (velo_pt2d->x() >= camera_bbox.xmin &&
        velo_pt2d->x() < camera_bbox.xmax &&
        velo_pt2d->y() >= camera_bbox.ymin &&
        velo_pt2d->y() < camera_bbox.ymax) {
      continue;
    }
    Eigen::Vector2d diff;
    diff.x() = std::max(0.0, camera_bbox.xmin - velo_pt2d->x());
    diff.x() = std::max(diff.x(), velo_pt2d->x() - camera_bbox.xmax);
    diff.y() = std::max(0.0, camera_bbox.ymin - velo_pt2d->y());
    diff.y() = std::max(diff.y(), velo_pt2d->y() - camera_bbox.ymax);
    mean_pixel_dist += diff;
  }
  mean_pixel_dist /= static_cast<double>(object->Size());
  ADEBUG << "mean_pixel_dist is: " << mean_pixel_dist;
  // normalize according to box size
  Eigen::Vector2d box_size = Eigen::Vector2d(
      camera_bbox.xmax - camera_bbox.xmin, camera_bbox.ymax - camera_bbox.ymin);
  mean_pixel_dist.array() /= box_size.array();
  // assuming the normalized distance obeys gauss distribution
  double square_norm_mean_pixel_dist =
      mean_pixel_dist.x() * mean_pixel_dist.x() / x_std_dev / x_std_dev +
      mean_pixel_dist.y() * mean_pixel_dist.y() / y_std_dev / y_std_dev;
  // use chi-square distribution. Cauchy may be more reasonable.
  double location_similarity =
      1 - ChiSquaredCdf2TableFun(square_norm_mean_pixel_dist);
  // for numerical stability
  location_similarity = std::max(min_p, std::min(max_p, location_similarity));
  return location_similarity;
}

ComputePtsBoxShapeSimilarity()

double apollo::perception::fusion::ComputePtsBoxShapeSimilarity	(	const ProjectionCachePtr &	cache,
		const ProjectionCacheObject *	object,
		const base::BBox2DF &	camera_bbox
	)

在文件 track_object_similarity.cc 第 108 行定义.

                                                                    {
  static const double min_p = 1e-3;
  static const double max_p = 1 - 1e-3;
  double x_std_dev = 0.3;
  double y_std_dev = 0.4;
  if (object->Empty()) {
    return min_p;
  }
  // compute 2d bbox size of camera & velo
  Eigen::Vector2d camera_box_size = Eigen::Vector2d(
      camera_bbox.xmax - camera_bbox.xmin, camera_bbox.ymax - camera_bbox.ymin);
  // handled one point case
  base::BBox2DF velo_projection_bbox = object->GetBox();
  Eigen::Vector2d velo_box_size = camera_box_size / 10;
  velo_box_size.x() =
      std::max(static_cast<float>(velo_box_size.x()),
               velo_projection_bbox.xmax - velo_projection_bbox.xmin);
  velo_box_size.y() =
      std::max(static_cast<float>(velo_box_size.y()),
               velo_projection_bbox.ymax - velo_projection_bbox.ymin);
  // compute normalized box size diff
  Eigen::Vector2d mean_box_size = (camera_box_size + velo_box_size) / 2;
  Eigen::Vector2d box_size_diff =
      (velo_box_size - camera_box_size).array() / mean_box_size.array();
  // assuming the normalized distance obeys gauss distribution
  double square_norm_box_size_diff =
      box_size_diff.x() * box_size_diff.x() / x_std_dev / x_std_dev +
      box_size_diff.y() * box_size_diff.y() / y_std_dev / y_std_dev;
  ADEBUG << "camera_box_size@(" << camera_box_size(0) << ","
         << camera_box_size(1) << "); "
         << "velo_box_size@(" << velo_box_size.x() << "," << velo_box_size.y()
         << "); "
         << "box_size_diff@(" << box_size_diff.x() << "," << box_size_diff.y()
         << "); "
         << "square_norm_box_size_diff@" << square_norm_box_size_diff;
  // use chi-square distribution. Cauchy may be more reasonable.
  double shape_similarity =
      1 - ChiSquaredCdf2TableFun(square_norm_box_size_diff);
  // for numerical stability
  shape_similarity = std::max(min_p, std::min(max_p, shape_similarity));
  return shape_similarity;
}

ComputePtsBoxSimilarity()

double apollo::perception::fusion::ComputePtsBoxSimilarity	(	const ProjectionCachePtr &	cache,
		const ProjectionCacheObject *	object,
		const base::BBox2DF &	camera_bbox
	)

在文件 track_object_similarity.cc 第 158 行定义.

                                                               {
  double location_similarity =
      ComputePtsBoxLocationSimilarity(cache, object, camera_bbox);
  double shape_similarity =
      ComputePtsBoxShapeSimilarity(cache, object, camera_bbox);
  double fused_similarity =
      FuseTwoProbabilities(location_similarity, shape_similarity);
  ADEBUG << "fused_similarity@" << fused_similarity << ", location_similarity@"
         << location_similarity << ", shape_similarity@" << shape_similarity;
  return fused_similarity;
}

ComputeRadarCameraHSimilarity()

double apollo::perception::fusion::ComputeRadarCameraHSimilarity	(	const SensorObjectConstPtr &	radar,
		const SensorObjectConstPtr &	camera,
		const double	size_y,
		const EigenVector< Eigen::Vector2d > &	radar_box2d_vertices,
		const HSimilarityParams &	params
	)

calculate the h similarity between radar and camera

参数

radar
camera
size_y
radar_box2d_vertices
params

返回

double

在文件 track_object_similarity.cc 第 209 行定义.

                                     {
  const double camera_height = camera->GetBaseObject()->size(2);
  double height_similarity = params.initial_similarity_;
  if (camera_height > kFloatEpsilon) {
    double min_height_diff = std::numeric_limits<double>::max();
    for (size_t i = 0; i < 4; ++i) {
      double img_car_height = std::abs(radar_box2d_vertices[i + 4].y() -
                                       radar_box2d_vertices[i].y());
      min_height_diff =
          std::min(std::abs(img_car_height - size_y), min_height_diff);
    }
    min_height_diff /= size_y;
    double normalized_min_height_diff = min_height_diff * min_height_diff /
                                        params.diff_std_dev_ /
                                        params.diff_std_dev_;
    height_similarity = 1 - ChiSquaredCdf1TableFun(normalized_min_height_diff);
    height_similarity = ScalePositiveProbability(height_similarity,
                                                 params.scale_positive_max_p_,
                                                 params.scale_positive_th_p_);
  }
  return height_similarity;
}

ComputeRadarCameraLocSimilarity()

double apollo::perception::fusion::ComputeRadarCameraLocSimilarity	(	const Eigen::Vector3d &	radar_ct,
		const SensorObjectConstPtr &	camera,
		const Eigen::Matrix4d &	world2camera_pose,
		const LocSimilarityParams &	params
	)

calculate the loc similarity between radar and camera

参数

radar_ct
camera
world2camera_pose
params

返回

double

在文件 track_object_similarity.cc 第 257 行定义.

                                                                          {
  Eigen::Vector3d camera_ct = camera->GetBaseObject()->center;
  Eigen::Vector3d camera_ct_c =
      (world2camera_pose * camera_ct.homogeneous()).head(3);
  double ct_diff = (radar_ct - camera_ct).norm();
  ct_diff = ct_diff / camera_ct_c.z();
  double ct_similarity = WelshVarLossFun(ct_diff, params.welsh_loss_thresh_,
                                         params.welsh_loss_scale_);
  ct_similarity = ScalePositiveProbability(
      ct_similarity, params.scale_positive_max_p_, params.scale_positive_th_p_);
  return ct_similarity;
}

ComputeRadarCameraVelocitySimilarity()

double apollo::perception::fusion::ComputeRadarCameraVelocitySimilarity	(	const SensorObjectConstPtr &	radar,
		const SensorObjectConstPtr &	camera
	)

calculate the velocity similarity between radar and camera

参数

radar
camera

返回

double

在文件 track_object_similarity.cc 第 273 行定义.

                                                                           {
  Eigen::Vector3f radar_velocity = radar->GetBaseObject()->velocity;
  Eigen::Vector3f camera_velocity = camera->GetBaseObject()->velocity;
 
  float scalar_radar_velocity = radar_velocity.norm();
  float scalar_camera_velocity = camera_velocity.norm();
  if (std::max(scalar_radar_velocity, scalar_camera_velocity) > 2) {
    float diff_velocity = (radar_velocity - camera_velocity).norm() / 2;
    float diff_velocity_ratio =
        diff_velocity / std::max(scalar_camera_velocity, scalar_radar_velocity);
    const float velocity_std = 0.15f;
    const float max_velocity_p = 0.9f;
    const float th_velocity_p = 0.5f;
    float velocity_score = static_cast<float>(
        1 - ChiSquaredCdf1TableFun(diff_velocity_ratio * diff_velocity_ratio /
                                   velocity_std / velocity_std));
    velocity_score = static_cast<float>(ScalePositiveProbability(
        velocity_score, max_velocity_p, th_velocity_p));
    return velocity_score;
  } else {
    return 0.5;
  }
}

ComputeRadarCameraWSimilarity()

double apollo::perception::fusion::ComputeRadarCameraWSimilarity	(	const SensorObjectConstPtr &	radar,
		const double	width,
		const double	size_x,
		const EigenVector< Eigen::Vector2d > &	radar_box2d_vertices,
		const WSimilarityParams &	params
	)

calculate the w similarity between radar and camera

参数

radar
width
size_x
radar_box2d_vertices
params

返回

double

在文件 track_object_similarity.cc 第 235 行定义.

                                     {
  std::vector<double> radar_box2d_xs = {
      radar_box2d_vertices[0].x(), radar_box2d_vertices[1].x(),
      radar_box2d_vertices[2].x(), radar_box2d_vertices[3].x()};
  for (double& x : radar_box2d_xs) {
    x = Bound(x, width, 0.0);
  }
  auto min_max_xs =
      std::minmax_element(radar_box2d_xs.begin(), radar_box2d_xs.end());
  double radar_box2d_width = *min_max_xs.second - *min_max_xs.first;
  double width_diff = std::abs(radar_box2d_width - size_x) / size_x;
  double normalized_width_diff =
      width_diff * width_diff / params.diff_std_dev_ / params.diff_std_dev_;
  double width_similarity = 1 - ChiSquaredCdf1TableFun(normalized_width_diff);
  width_similarity = BoundedScalePositiveProbability(
      width_similarity, params.bounded_scale_positive_max_p_,
      params.bounded_scale_positive_min_p_);
  return width_similarity;
}

ComputeRadarCameraXSimilarity()

double apollo::perception::fusion::ComputeRadarCameraXSimilarity	(	const double	velo_ct_x,
		const double	camera_ct_x,
		const double	size_x,
		const XSimilarityParams &	params
	)

在文件 track_object_similarity.cc 第 177 行定义.

                                                                      {
  double x_diff = std::abs(velo_ct_x - camera_ct_x) / size_x;
  double x_similarity = WelshVarLossFun(x_diff, params.welsh_loss_thresh_,
                                        params.welsh_loss_scale_);
  x_similarity = ScalePositiveProbability(
      x_similarity, params.scale_positive_max_p_, params.scale_positive_th_p_);
  return x_similarity;
}

ComputeRadarCameraYSimilarity()

double apollo::perception::fusion::ComputeRadarCameraYSimilarity	(	const double	velo_ct_y,
		const double	camera_ct_y,
		const double	size_y,
		const YSimilarityParams &	params
	)

calculate the y similarity between radar and camera

参数

velo_ct_y
camera_ct_y
size_y
params

返回

double

在文件 track_object_similarity.cc 第 188 行定义.

                                                                      {
  // double y_diff =
  //     std::abs(velo_ct_y - camera_ct_y + size_y * params.smooth_factor_) /
  //     size_y;
 
  double y_diff = std::max(std::abs(velo_ct_y - camera_ct_y) -
                               size_y * params.smooth_factor_,
                           0.0) /
                  size_y;
 
  double normalized_y_diff =
      y_diff * y_diff / params.diff_std_dev_ / params.diff_std_dev_;
  double y_similarity = 1 - ChiSquaredCdf1TableFun(normalized_y_diff);
  y_similarity = BoundedScalePositiveProbability(
      y_similarity, params.bounded_scale_positive_max_p_,
      params.bounded_scale_positive_min_p_);
  return y_similarity;
}

CYBER_REGISTER_COMPONENT()

apollo::perception::fusion::CYBER_REGISTER_COMPONENT

(

MultiSensorFusionComponent

)

extract_vector()

template<typename T >

void apollo::perception::fusion::extract_vector	(	const std::vector< T > &	vec,
		const std::vector< size_t > &	subset_inds,
		std::vector< T > *	sub_vec
	)

在文件 hm_tracks_objects_match.cc 第 37 行定义.

                                           {
  sub_vec->reserve(subset_inds.size());
  sub_vec->clear();
  for (auto subset_ind : subset_inds) {
    sub_vec->push_back(vec[subset_ind]);
  }
}

FuseMultipleProbabilities()

double apollo::perception::fusion::FuseMultipleProbabilities

(

const std::vector< double > &

probs

)

在文件 probabilities.cc 第 69 行定义.

                                                                 {
  std::vector<double> log_odd_probs = probs;
  auto prob_to_log_odd = [](double p) {
    p = std::max(std::min(p, 1 - 1e-6), 1e-6);
    return std::log(p / (1 - p));
  };
  auto log_odd_to_prob = [](double log_odd_p) {
    double tmp = std::exp(log_odd_p);
    return tmp / (tmp + 1);
  };
  for (auto& log_odd_prob : log_odd_probs) {
    log_odd_prob = prob_to_log_odd(log_odd_prob);
  }
  double log_odd_probs_sum =
      std::accumulate(log_odd_probs.begin(), log_odd_probs.end(), 0.0);
  return log_odd_to_prob(log_odd_probs_sum);
}

FuseTwoProbabilities()

double apollo::perception::fusion::FuseTwoProbabilities	(	double	prob1,
		double	prob2
	)

在文件 probabilities.cc 第 62 行定义.

                                                        {
  double prob = (prob1 * prob2) / (2 * prob1 * prob2 + 1 - prob1 - prob2);
  return prob;
}

FUSION_REGISTER_FUSIONSYSTEM() [1/2]

apollo::perception::fusion::FUSION_REGISTER_FUSIONSYSTEM

(

DummyFusionSystem

)

FUSION_REGISTER_FUSIONSYSTEM() [2/2]

apollo::perception::fusion::FUSION_REGISTER_FUSIONSYSTEM

(

ProbabilisticFusion

)

GetObjectEightVertices() [1/2]

void apollo::perception::fusion::GetObjectEightVertices	(	std::shared_ptr< const base::Object >	obj,
		apollo::common::EigenVector< Eigen::Vector3d > *	vertices
	)

GetObjectEightVertices() [2/2]

void apollo::perception::fusion::GetObjectEightVertices	(	std::shared_ptr< const base::Object >	obj,
		EigenVector< Eigen::Vector3d > *	vertices
	)

在文件 camera_util.cc 第 31 行定义.

                                                                  {
  vertices->clear();
  vertices->resize(8);
  Eigen::Vector3d center = obj->center;
  Eigen::Vector2d dir = obj->direction.head(2).cast<double>();
  dir.normalize();
  Eigen::Vector2d orth_dir(-dir.y(), dir.x());
  Eigen::Vector2d delta_x = dir * obj->size(0) * 0.5;
  Eigen::Vector2d delta_y = orth_dir * obj->size(1) * 0.5;
 
  // lower four points
  center.z() -= obj->size(2) * 0.5;
  (*vertices)[0] = center, (*vertices)[0].head(2) += (-delta_x - delta_y);
  (*vertices)[1] = center, (*vertices)[1].head(2) += (-delta_x + delta_y);
  (*vertices)[2] = center, (*vertices)[2].head(2) += (delta_x + delta_y);
  (*vertices)[3] = center, (*vertices)[3].head(2) += (delta_x - delta_y);
  // upper four points
  (*vertices)[4] = (*vertices)[0], (*vertices)[4].z() += obj->size(2);
  (*vertices)[5] = (*vertices)[1], (*vertices)[5].z() += obj->size(2);
  (*vertices)[6] = (*vertices)[2], (*vertices)[6].z() += obj->size(2);
  (*vertices)[7] = (*vertices)[3], (*vertices)[7].z() += obj->size(2);
}

IsCamera()

bool apollo::perception::fusion::IsCamera

(

const SensorObjectConstPtr &

obj

)

在文件 sensor_object.cc 第 96 行定义.

                                               {
  base::SensorType type = obj->GetSensorType();
  return algorithm::SensorManager::Instance()->IsCamera(type);
}

IsLidar()

bool apollo::perception::fusion::IsLidar

(

const SensorObjectConstPtr &

obj

)

在文件 sensor_object.cc 第 86 行定义.

                                              {
  base::SensorType type = obj->GetSensorType();
  return algorithm::SensorManager::Instance()->IsLidar(type);
}

IsObjectEightVerticesAllBehindCamera()

bool apollo::perception::fusion::IsObjectEightVerticesAllBehindCamera	(	const std::shared_ptr< const base::Object > &	obj,
		const Eigen::Matrix4d &	world2camera_pose,
		base::BaseCameraModelPtr	camera_model
	)

在文件 camera_util.cc 第 72 行定义.

                                         {
  EigenVector<Eigen::Vector3d> vertices(8);
  GetObjectEightVertices(obj, &vertices);
  Eigen::Vector2d pt2d;
  for (const auto& vertice : vertices) {
    if ((Pt3dToCamera2d(vertice, world2camera_pose, camera_model, &pt2d))) {
      return false;
    }
  }
  return true;
}

IsPtInFrustum()

template<typename VectorType >

bool apollo::perception::fusion::IsPtInFrustum	(	const VectorType &	pt,
		double	width,
		double	height
	)

Is point lies in the frustum

模板参数

VectorType

参数

pt	point
width
height

返回

true

false

在文件 camera_util.h 第 49 行定义.

                                                                      {
  if (pt[0] < 0 || pt[0] > width || pt[1] < 0 || pt[1] > height) {
    return false;
  }
  return true;
}

IsRadar()

bool apollo::perception::fusion::IsRadar

(

const SensorObjectConstPtr &

obj

)

在文件 sensor_object.cc 第 91 行定义.

                                              {
  base::SensorType type = obj->GetSensorType();
  return algorithm::SensorManager::Instance()->IsRadar(type);
}

ObjectInCameraView()

float apollo::perception::fusion::ObjectInCameraView	(	SensorObjectConstPtr	sensor_object,
		base::BaseCameraModelPtr	camera_model,
		const Eigen::Affine3d &	camera_sensor2world_pose,
		double	camera_ts,
		double	camera_max_dist,
		bool	motion_compensation,
		bool	all_in
	)

在文件 camera_util.cc 第 87 行定义.

                                                                {
  constexpr float kFloatEpsilon = std::numeric_limits<float>::epsilon();
  float in_view_ratio = 0.0f;
  Eigen::Matrix4d world2sensor_pose =
      camera_sensor2world_pose.matrix().inverse();
  if (!world2sensor_pose.allFinite()) {
    return in_view_ratio;
  }
  double width = static_cast<double>(camera_model->get_width());
  double height = static_cast<double>(camera_model->get_height());
 
  double time_diff =
      camera_ts - sensor_object->GetBaseObject()->latest_tracked_time;
  Eigen::Vector3f offset =
      sensor_object->GetBaseObject()->velocity * static_cast<float>(time_diff);
  if (!motion_compensation) {
    offset.setZero();
  }
  // 2.compute distance
  const auto& cloud =
      sensor_object->GetBaseObject()->lidar_supplement.cloud_world;
  Eigen::Vector3d center = sensor_object->GetBaseObject()->center;
  Eigen::Vector2d center2d;
  if (!Pt3dToCamera2d(center, world2sensor_pose, camera_model, &center2d)) {
    return in_view_ratio;
  }
  double obj_length = sensor_object->GetBaseObject()->size(0);
  double obj_width = sensor_object->GetBaseObject()->size(1);
  double obj_height = sensor_object->GetBaseObject()->size(2);
  if (cloud.size() > 0) {
    // use point cloud
    int point_num = static_cast<int>(cloud.size());
    int in_view_point_num = 0;
    for (int i = 0; i < point_num; ++i) {
      const auto& pt = cloud.at(i);
      Eigen::Vector3d pt3d(pt.x + offset[0], pt.y + offset[1],
                           pt.z + offset[2]);
      Eigen::Vector2d pt2d;
      bool flag = Pt3dToCamera2d(pt3d, world2sensor_pose, camera_model, &pt2d);
      if (flag && IsPtInFrustum(pt2d, width, height)) {
        ++in_view_point_num;
      }
    }
    in_view_ratio = static_cast<float>(in_view_point_num / point_num);
    if (all_in) {
      in_view_ratio = (in_view_point_num == point_num) ? 1.0 : 0.0;
    }
  } else if (obj_width > kFloatEpsilon && obj_height > kFloatEpsilon &&
             obj_length > kFloatEpsilon) {
    // use object box
    EigenVector<Eigen::Vector3d> box3d_vs(8);
    GetObjectEightVertices(sensor_object->GetBaseObject(), &box3d_vs);
    EigenVector<Eigen::Vector2f> box2d_vs;
    box2d_vs.reserve(box3d_vs.size());
    for (const auto& box3d_v : box3d_vs) {
      Eigen::Vector2d pt2d;
      bool flag = Pt3dToCamera2d(box3d_v + offset.cast<double>(),
                                 world2sensor_pose, camera_model, &pt2d);
      if (!flag) {
        return in_view_ratio;
      }
      box2d_vs.push_back(pt2d.cast<float>());
    }
    Eigen::Vector2d top_left;
    Eigen::Vector2d bottom_right;
    top_left.setConstant(std::numeric_limits<double>::max());
    bottom_right = -top_left;
    for (const auto& box2d_v : box2d_vs) {
      top_left = top_left.cwiseMin(box2d_v.cast<double>());
      bottom_right = bottom_right.cwiseMax(box2d_v.cast<double>());
    }
    Eigen::Vector2d box_size = bottom_right - top_left;
    Eigen::Vector2d bound_top_left =
        top_left.cwiseMax(Eigen::Vector2d(0.0, 0.0));
    Eigen::Vector2d bound_bottom_right =
        bottom_right.cwiseMin(Eigen::Vector2d(width, height));
    Eigen::Vector2d bound_box_size = bound_bottom_right - bound_top_left;
    if ((bound_box_size.array() > 0.0).all()) {
      in_view_ratio =
          static_cast<float>(bound_box_size.prod() / box_size.prod());
    } else {
      in_view_ratio = 0.0;
    }
    if (all_in) {
      in_view_ratio = std::abs(1.0 - in_view_ratio) <= 1e-6 ? 1.0 : 0.0;
    }
  } else {  // use center point
    if (!((center2d.array() > 0.0).all())) {
      in_view_ratio = 0.0;
    } else if (center2d.x() > width || center2d.y() > height) {
      in_view_ratio = 0.0;
    } else {
      in_view_ratio = 1.0;
    }
  }
  // compute distance score, when object is too far from camera
  // the camera is hard to detect object
  // maximum camera detection distance parameters
  const double max_dist = camera_max_dist;
  // 1 nearly 2m buffer
  const double dist_slope = 0.25;
  auto sigmoid_like_fun = [max_dist, dist_slope](double obj_dist) {
    double x = obj_dist - max_dist;
    return 0.5 - 0.5 * x * dist_slope /
                     std::sqrt(1 + x * x * dist_slope * dist_slope);
  };
  Eigen::Vector4d center3d_local = world2sensor_pose * center.homogeneous();
  double dist_to_camera = center3d_local.z();
  return static_cast<float>(in_view_ratio * sigmoid_like_fun(dist_to_camera));
}

operator*()

Dst apollo::perception::fusion::operator*	(	const Dst &	dst,
		double	w
	)

参数

dst_evidence
w

返回

Dst

在文件 dst_evidence.cc 第 396 行定义.

                                        {
  dst.SelfCheck();
  Dst res(dst.app_name_);
  // check w
  if (w < 0.0 || w > 1.0) {
    AERROR << boost::format(
                  "the weight of bba %lf is not valid, return default bba") %
                  w;
    return res;
  }
  size_t fod_loc = dst.dst_data_ptr_->fod_loc_;
  std::vector<double> &resbba_vec_ = res.bba_vec_;
  const std::vector<double> &bba_vec = dst.bba_vec_;
  for (size_t i = 0; i < resbba_vec_.size(); ++i) {
    if (i == fod_loc) {
      resbba_vec_[i] = 1.0 - w + w * bba_vec[i];
    } else {
      resbba_vec_[i] = w * bba_vec[i];
    }
  }
  return res;
}

operator+()

Dst apollo::perception::fusion::operator+	(	const Dst &	lhs,
		const Dst &	rhs
	)

参数

lhs
rhs

返回

Dst

在文件 dst_evidence.cc 第 370 行定义.

                                              {
  CHECK_EQ(lhs.app_name_, rhs.app_name_)
      << boost::format("lhs Dst(%s) is not equal to rhs Dst(%s)") %
             lhs.app_name_ % rhs.app_name_;
  lhs.SelfCheck();
  rhs.SelfCheck();
  Dst res(lhs.app_name_);
  std::vector<double> &resbba_vec_ = res.bba_vec_;
  const auto &combination_relations = lhs.dst_data_ptr_->combination_relations_;
  for (size_t i = 0; i < resbba_vec_.size(); ++i) {
    const auto &combination_pairs = combination_relations[i];
    // AINFO << "pairs size: " << combination_pairs.size();
    double &belief_mass = resbba_vec_[i];
    belief_mass = 0.0;
    for (auto combination_pair : combination_pairs) {
      // AINFO << boost::format("(%d %d)") % combination_pair.first
      //     % combination_pair.second;
      belief_mass += lhs.GetIndBfmass(combination_pair.first) *
                     rhs.GetIndBfmass(combination_pair.second);
    }
    // AINFO << boost::format("belief_mass: %lf") % belief_mass;
  }
  res.Normalize();
  return res;
}

PERCEPTION_REGISTER_REGISTERER() [1/3]

apollo::perception::fusion::PERCEPTION_REGISTER_REGISTERER

(

BaseDataAssociation

)

PERCEPTION_REGISTER_REGISTERER() [2/3]

apollo::perception::fusion::PERCEPTION_REGISTER_REGISTERER

(

BaseFusionSystem

)

PERCEPTION_REGISTER_REGISTERER() [3/3]

apollo::perception::fusion::PERCEPTION_REGISTER_REGISTERER

(

BaseGatekeeper

)

Pt3dToCamera2d()

bool apollo::perception::fusion::Pt3dToCamera2d	(	const Eigen::Vector3d &	pt3d,
		const Eigen::Matrix4d &	world2camera_pose,
		base::BaseCameraModelPtr	camera_model,
		Eigen::Vector2d *	pt2d
	)

在文件 camera_util.cc 第 55 行定义.

                                         {
  Eigen::Vector4d local_pt = static_cast<Eigen::Matrix<double, 4, 1, 0, 4, 1>>(
      world2camera_pose * Eigen::Vector4d(pt3d(0), pt3d(1), pt3d(2), 1));
  if (local_pt[2] > 0) {
    *pt2d =
        (camera_model->Project(Eigen::Vector3f(
             static_cast<float>(local_pt[0]), static_cast<float>(local_pt[1]),
             static_cast<float>(local_pt[2]))))
            .cast<double>();
    return true;
  }
  return false;
}

ScalePositiveProbability()

double apollo::perception::fusion::ScalePositiveProbability	(	double	p,
		double	max_p,
		double	th_p
	)

在文件 probabilities.cc 第 36 行定义.

                                                                     {
  if (p <= th_p) {
    return p;
  }
  p = (p - th_p) * (max_p - th_p) / (1 - th_p) + th_p;
  return p;
}

vector2string()

template<typename Type >

std::string apollo::perception::fusion::vector2string

(

const std::vector< Type > &

values

)

在文件 dst_type_fusion.cc 第 41 行定义.

                                                       {
  return absl::StrCat("(", absl::StrJoin(values, " "), ")");
}

WelshVarLossFun()

double apollo::perception::fusion::WelshVarLossFun	(	double	dist,
		double	th,
		double	scale
	)

在文件 probabilities.cc 第 46 行定义.

                                                             {
  double p = 1e-6;
  if (dist < th) {
    p = 1 - 1e-6;
  } else {
    dist -= th;
    dist /= scale;
    p = std::exp(-dist * dist);
  }
  return p;
}

变量说明

EIGEN_ALIGN16

struct apollo::perception::fusion::SensorFrameHeader apollo::perception::fusion::EIGEN_ALIGN16