apollo::perception::fusion::KalmanMotionFusion类 参考

#include <kalman_motion_fusion.h>

类 apollo::perception::fusion::KalmanMotionFusion 继承关系图:

apollo::perception::fusion::KalmanMotionFusion 的协作图:

Public 成员函数
	KalmanMotionFusion (TrackPtr track)
	~KalmanMotionFusion ()=default
bool	Init () override
	Init base data association config
void	UpdateWithMeasurement (const SensorObjectConstPtr &measurement, double target_timestamp) override
	update track motion state with measurement
void	UpdateWithoutMeasurement (const std::string &sensor_id, double measurement_timestamp, double target_timestamp) override
	update track motion state without measurement
std::string	Name () const override
	The name of BaseMotionFusion
void	GetStates (Eigen::Vector3d *anchor_point, Eigen::Vector3d *velocity)
	Get states
Public 成员函数 继承自 apollo::perception::fusion::BaseMotionFusion
	BaseMotionFusion (TrackPtr track)
virtual	~BaseMotionFusion ()=default

额外继承的成员函数
Protected 成员函数 继承自 apollo::perception::fusion::BaseMotionFusion
	DISALLOW_COPY_AND_ASSIGN (BaseMotionFusion)
Protected 属性 继承自 apollo::perception::fusion::BaseMotionFusion
TrackPtr	track_ref_

详细描述

在文件 kalman_motion_fusion.h 第 31 行定义.

构造及析构函数说明

KalmanMotionFusion()

apollo::perception::fusion::KalmanMotionFusion::KalmanMotionFusion ( TrackPtr  track )

inlineexplicit

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

: BaseMotionFusion(track) {}

~KalmanMotionFusion()

apollo::perception::fusion::KalmanMotionFusion::~KalmanMotionFusion ( )

default

成员函数说明

GetStates()

void apollo::perception::fusion::KalmanMotionFusion::GetStates	(	Eigen::Vector3d *	anchor_point,
		Eigen::Vector3d *	velocity
	)

Get states

参数

anchor_point
velocity

在文件 kalman_motion_fusion.cc 第 103 行定义.

                                                            {
  *anchor_point = fused_anchor_point_;
  *velocity = fused_velocity_;
}

Init()

bool apollo::perception::fusion::KalmanMotionFusion::Init ( )

overridevirtual

Init base data association config

返回

true

false

实现了 apollo::perception::fusion::BaseMotionFusion.

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

                              {
  if (track_ref_ == nullptr) {
    return false;
  }
  if (track_ref_->GetLatestLidarObject() != nullptr) {
    filter_init_ = InitFilter(track_ref_->GetLatestLidarObject());
  } else if (track_ref_->GetLatestRadarObject() != nullptr) {
    filter_init_ = InitFilter(track_ref_->GetLatestRadarObject());
  }
  return true;
}

Name()

std::string apollo::perception::fusion::KalmanMotionFusion::Name ( ) const

inlineoverridevirtual

The name of BaseMotionFusion

返回

std::string

实现了 apollo::perception::fusion::BaseMotionFusion.

在文件 kalman_motion_fusion.h 第 53 行定义.

{ return "KalmamnMotionFusion"; }

UpdateWithMeasurement()

void apollo::perception::fusion::KalmanMotionFusion::UpdateWithMeasurement ( const SensorObjectConstPtr &  measurement, double  target_timestamp  )

overridevirtual

update track motion state with measurement

参数

measurement
target_timestamp

实现了 apollo::perception::fusion::BaseMotionFusion.

在文件 kalman_motion_fusion.cc 第 134 行定义.

                                                                      {
  fused_anchor_point_ =
      measurement->GetBaseObject()->anchor_point.cast<double>();
  fused_velocity_ = measurement->GetBaseObject()->velocity.cast<double>();
  fused_acceleration_ =
      measurement->GetBaseObject()->acceleration.cast<double>();
  center_uncertainty_ = measurement->GetBaseObject()->center_uncertainty;
  velo_uncertainty_ = measurement->GetBaseObject()->velocity_uncertainty;
  acc_uncertainty_ = measurement->GetBaseObject()->acceleration_uncertainty;
 
  double time_diff = measurement->GetTimestamp() -
                     track_ref_->GetFusedObject()->GetTimestamp();
  bool is_lidar = IsLidar(measurement);
  bool is_radar = IsRadar(measurement);
  bool is_camera = IsCamera(measurement);
  SensorObjectConstPtr lidar_ptr = track_ref_->GetLatestLidarObject();
  SensorObjectConstPtr radar_ptr = track_ref_->GetLatestRadarObject();
  SensorObjectConstPtr camera_ptr = track_ref_->GetLatestCameraObject();
  // Motion fusion
  if (is_lidar || is_radar || is_camera) {
    if (filter_init_) {
      MotionFusionWithMeasurement(measurement, time_diff);
    } else {
      filter_init_ = InitFilter(measurement);
    }
    if (!filter_init_) {
      // No kalman result, no matter which sensortype
      // of measurement, use measurement's
      // anchor point and velocity
      return;
    }
  }
  // shape & location fusion
  if (is_lidar || is_radar) {
    if (is_lidar || (is_radar && lidar_ptr == nullptr)) {
      // 1) measurement is lidar: use lidar's anchor point
      //    , use fused velocity
      // 2) measurement is radar, and has no history lidar:
      // use radar's anchor point, use fused velocity
      fused_velocity_(0) = kalman_filter_.GetStates()(2);
      fused_velocity_(1) = kalman_filter_.GetStates()(3);
    }
    if (is_radar && lidar_ptr != nullptr) {
      // measurement is radar, has history lidar:
      // use fused achor point, use fused velocity
      fused_anchor_point_(0) = kalman_filter_.GetStates()(0);
      fused_anchor_point_(1) = kalman_filter_.GetStates()(1);
      fused_velocity_(0) = kalman_filter_.GetStates()(2);
      fused_velocity_(1) = kalman_filter_.GetStates()(3);
    }
  } else if (is_camera) {
    if (filter_init_) {
      if (lidar_ptr != nullptr) {
        // measurement is camera, has history lidar
        // use fused position, use fused velocity
        fused_anchor_point_(0) = kalman_filter_.GetStates()(0);
        fused_anchor_point_(1) = kalman_filter_.GetStates()(1);
        fused_velocity_(0) = kalman_filter_.GetStates()(2);
        fused_velocity_(1) = kalman_filter_.GetStates()(3);
      } else if (radar_ptr != nullptr) {
        // measurement is camera, has no history lidar, but
        // has history radar, use history radar's position
        // use fused velocity
        fused_anchor_point_ =
            radar_ptr->GetBaseObject()->anchor_point.cast<double>();
        fused_velocity_(0) = kalman_filter_.GetStates()(2);
        fused_velocity_(1) = kalman_filter_.GetStates()(3);
      } else {
        // measurement is camera, has no history lidar
        // or radar, use measurement's anchor point and
        // velocity, do nothing(at the beginning of this
        // function, fused velocity/anchor point has been
        // set to as same as measurement)
      }
    }
  }
  fused_acceleration_(0) = kalman_filter_.GetStates()(4);
  fused_acceleration_(1) = kalman_filter_.GetStates()(5);
  // Originally, we would reset filter_init_ to false, when there is no
  // valid lidar & radar measurement. now, as the quality of estimation
  // of camera improved, this step is not needed.
  UpdateMotionState();
}

UpdateWithoutMeasurement()

void apollo::perception::fusion::KalmanMotionFusion::UpdateWithoutMeasurement ( const std::string &  sensor_id, double  measurement_timestamp, double  target_timestamp  )

overridevirtual

update track motion state without measurement

参数

sensor_id
measurement_timestamp
target_timestamp

实现了 apollo::perception::fusion::BaseMotionFusion.

在文件 kalman_motion_fusion.cc 第 109 行定义.

                                                                           {
  SensorObjectConstPtr lidar_ptr = track_ref_->GetLatestLidarObject();
  SensorObjectConstPtr radar_ptr = track_ref_->GetLatestRadarObject();
  SensorObjectConstPtr camera_ptr = track_ref_->GetLatestCameraObject();
  bool is_alive =
      (lidar_ptr != nullptr || radar_ptr != nullptr || camera_ptr != nullptr);
  if (filter_init_ && is_alive) {
    double time_diff =
        measurement_timestamp - track_ref_->GetFusedObject()->GetTimestamp();
    MotionFusionWithoutMeasurement(time_diff);
    fused_anchor_point_(0) = kalman_filter_.GetStates()(0);
    fused_anchor_point_(1) = kalman_filter_.GetStates()(1);
    fused_velocity_(0) = kalman_filter_.GetStates()(2);
    fused_velocity_(1) = kalman_filter_.GetStates()(3);
    fused_acceleration_(0) = kalman_filter_.GetStates()(4);
    fused_acceleration_(1) = kalman_filter_.GetStates()(5);
  }
  // Originally, we would reset filter_init_ to false, when there is no
  // valid lidar & radar measurement. now, as the quality of estimation
  // of camera improved, this step is not needed.
  UpdateMotionState();
}

---

该类的文档由以下文件生成:

• modules/perception/multi_sensor_fusion/fusion/data_fusion/motion_fusion/kalman_motion_fusion/kalman_motion_fusion.h
• modules/perception/multi_sensor_fusion/fusion/data_fusion/motion_fusion/kalman_motion_fusion/kalman_motion_fusion.cc