apollo::perception::radar4d::MrfMotionFilter类 参考

#include <mrf_motion_filter.h>

类 apollo::perception::radar4d::MrfMotionFilter 继承关系图:

apollo::perception::radar4d::MrfMotionFilter 的协作图:

Public 成员函数
	MrfMotionFilter ()=default
virtual	~MrfMotionFilter ()=default
bool	Init (const MrfFilterInitOptions &options=MrfFilterInitOptions()) override
	Init mrf fitler
void	UpdateWithObject (const MrfFilterOptions &options, const MrfTrackDataConstPtr &track_data, TrackedObjectPtr new_object) override
	Updating motion filter with object
void	UpdateWithoutObject (const MrfFilterOptions &options, double timestamp, MrfTrackDataPtr track_data) override
	Updating motion filter without object
std::string	Name () const override
	Get class name
Public 成员函数 继承自 apollo::perception::radar4d::MrfBaseFilter
	MrfBaseFilter ()=default
virtual	~MrfBaseFilter ()=default

Protected 成员函数
void	InitializeTrackState (TrackedObjectPtr new_object)
	Initialize track state and store in the new object
void	KalmanFilterUpdateWithPartialObservation (const MrfTrackDataConstPtr &track_data, const TrackedObjectConstPtr &latest_object, TrackedObjectPtr new_object)
	Update state with kalman filter, constant acceleration motion model, only velocity measurement is observed
void	StateGainAdjustment (const MrfTrackDataConstPtr &track_data, const TrackedObjectConstPtr &latest_object, const TrackedObjectConstPtr &new_object, Eigen::Vector4d *gain)
	Adjust kalman state gain with several strategies
void	ConvergenceEstimationAndBoostUp (const MrfTrackDataConstPtr &track_data, const TrackedObjectConstPtr &latest_object, TrackedObjectPtr new_object)
	Estimate convergence confidence and boost up state
void	ComputeConvergenceConfidence (const MrfTrackDataConstPtr &track_data, TrackedObjectPtr new_object, bool velocity_source_is_belief=true)
	Compute convergence confidence
void	BoostupState (const MrfTrackDataConstPtr &track_data, TrackedObjectPtr new_object)
	Boost up state considering track history
void	ClipingState (TrackedObjectPtr object)
	Cliping state if is within noise level
void	OnlineCovarianceEstimation (const MrfTrackDataConstPtr &track_data, TrackedObjectPtr object)
	Estimate covariance considering history measurment
void	UpdateConverged (const MrfTrackDataConstPtr &track_data, TrackedObjectPtr object)
	Update convergence confidence
void	StateToBelief (TrackedObjectPtr object)
	Synchronize state to belief to keep consistency
void	BeliefToOutput (TrackedObjectPtr object)
	Copy belief to output

Protected 属性
const double	EPSION_TIME = 1e-3
const double	DEFAULT_FPS = 0.1
std::shared_ptr< MrfMotionMeasurement >	motion_measurer_
std::shared_ptr< MrfMotionRefiner >	motion_refiner_
bool	use_adaptive_ = true
bool	use_breakdown_ = true
bool	use_convergence_boostup_ = true
double	init_velocity_variance_ = 5.0
double	init_acceleration_variance_ = 10.0
double	measured_velocity_variance_ = 0.4
double	predict_variance_per_sqrsec_ = 50.0
size_t	boostup_history_size_minimum_ = 3
size_t	boostup_history_size_maximum_ = 6
double	converged_confidence_minimum_ = 0.5
double	noise_maximum_ = 0.1
double	trust_orientation_range_ = 40.0

详细描述

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

构造及析构函数说明

MrfMotionFilter()

apollo::perception::radar4d::MrfMotionFilter::MrfMotionFilter ( )

default

~MrfMotionFilter()

virtual apollo::perception::radar4d::MrfMotionFilter::~MrfMotionFilter ( )

virtualdefault

成员函数说明

BeliefToOutput()

void apollo::perception::radar4d::MrfMotionFilter::BeliefToOutput ( TrackedObjectPtr  object )

protected

Copy belief to output

参数

object

new object to be updated

在文件 mrf_motion_filter.cc 第 260 行定义.

                                                            {
  object->output_velocity = object->belief_velocity;
  object->output_velocity_uncertainty =
      object->belief_velocity_online_covariance;
}

BoostupState()

void apollo::perception::radar4d::MrfMotionFilter::BoostupState ( const MrfTrackDataConstPtr &  track_data, TrackedObjectPtr  new_object  )

protected

Boost up state considering track history

参数

track_data	history track data
new_object	new object to be updated

在文件 mrf_motion_filter.cc 第 401 行定义.

                                                                {
  // Compute min & max boosted velocity
  Eigen::Vector3d& new_obj_belief_velocity = new_object->belief_velocity;
  Eigen::Vector3d min_boosted_velocity = new_obj_belief_velocity;
  constexpr double kDoubleMax = std::numeric_limits<double>::max();
  double min_boosted_velocity_norm = kDoubleMax;
  Eigen::Vector3d max_boosted_velocity = new_obj_belief_velocity;
  double max_boosted_velocity_norm = kDoubleMax;
  Eigen::Vector3d project_dir = min_boosted_velocity;
  project_dir(2) = 0.0;
  project_dir.normalize();
 
  int boostup_need_history_size = new_object->boostup_need_history_size;
  int actual_boostup_history_size =
      static_cast<size_t>(boostup_need_history_size) >
              track_data->history_objects_.size()
          ? static_cast<int>(track_data->history_objects_.size())
          : boostup_need_history_size;
  int boostup_used_history_size = 0;
  auto& history_objects = track_data->history_objects_;
  auto cur_object_pair = history_objects.rbegin();
  TrackedObjectPtr cur_object = new_object;
  while (boostup_used_history_size < actual_boostup_history_size &&
         cur_object_pair != history_objects.rend()) {
    ++boostup_used_history_size;
    Eigen::Vector3d measured_velocity = cur_object->selected_measured_velocity;
    Eigen::Vector3d measured_velocity_on_project_dir =
        algorithm::Calculate2DXYProjectVector<double>(measured_velocity,
                                                   new_obj_belief_velocity);
    if (measured_velocity_on_project_dir.dot(new_obj_belief_velocity) <= 0) {
      measured_velocity_on_project_dir = Eigen::Vector3d::Zero();
    }
    double measured_velocity_norm_on_project_dir =
        measured_velocity_on_project_dir.norm();
    if (measured_velocity_norm_on_project_dir < min_boosted_velocity_norm) {
      min_boosted_velocity = measured_velocity_on_project_dir;
      min_boosted_velocity_norm = measured_velocity_norm_on_project_dir;
    }
    if (measured_velocity_norm_on_project_dir > max_boosted_velocity_norm) {
      max_boosted_velocity = measured_velocity_on_project_dir;
      max_boosted_velocity_norm = measured_velocity_norm_on_project_dir;
    }
    if (boostup_used_history_size > 1) {
      ++cur_object_pair;
    }
    if (cur_object_pair != history_objects.rend()) {
      cur_object = cur_object_pair->second;
    }
  }
  // Increase belief when belief less than min boosted velocity
  // Decrease belief when belief greater than max boosted velocity
  // now boosted_acceleration not used in original version, maybe use later
  if (min_boosted_velocity_norm > new_obj_belief_velocity.norm()) {
    // Eigen::Vector3d boosted_acceleration =
    // (min_boosted_velocity - new_obj_belief_velocity)/new_latest_time_diff_;
    new_obj_belief_velocity = min_boosted_velocity;
  } else if (max_boosted_velocity_norm < new_obj_belief_velocity.norm()) {
    // Eigen::Vector3d boosted_acceleration =
    // (max_boosted_velocity - new_obj_belief_velocity)/new_latest_time_diff_;
    new_obj_belief_velocity = max_boosted_velocity;
  }
  new_object->state.head<2>() = new_obj_belief_velocity.head<2>();
}

ClipingState()

void apollo::perception::radar4d::MrfMotionFilter::ClipingState ( TrackedObjectPtr  object )

protected

Cliping state if is within noise level

参数

object

new object to be updated

在文件 mrf_motion_filter.cc 第 266 行定义.

                                                          {
  if (object->state.head<2>().norm() < noise_maximum_) {
    object->state.setZero();
    object->belief_velocity_gain.setZero();
    object->state_covariance.block<2, 2>(0, 2).setZero();
    object->state_covariance.block<2, 2>(2, 0).setZero();
  }
}

ComputeConvergenceConfidence()

void apollo::perception::radar4d::MrfMotionFilter::ComputeConvergenceConfidence ( const MrfTrackDataConstPtr &  track_data, TrackedObjectPtr  new_object, bool  velocity_source_is_belief = true  )

protected

Compute convergence confidence

参数

track_data	history track data
new_object	new object to be updated
velocity_source_is_belief	whether using belief velocity or output velocity

在文件 mrf_motion_filter.cc 第 340 行定义.

                                    {
  // Compute convergence score list
  std::vector<double> convergence_score_list;
  int boostup_need_history_size = new_object->boostup_need_history_size;
  auto& history_objects = track_data->history_objects_;
 
  convergence_score_list.resize(boostup_need_history_size, 0.0);
  double base_convergence_noise = 2 * measured_velocity_variance_;
 
  // new object score
  Eigen::Vector3d velocity_residual =
      new_object->selected_measured_velocity -
      (velocity_source_is_belief ? new_object->belief_velocity
                                 : new_object->output_velocity);
  convergence_score_list[0] =
      base_convergence_noise /
      std::max(base_convergence_noise, velocity_residual.norm());
 
  // history objects score
  size_t visible_obj_idx = 1;
  for (auto cur_object_pair = history_objects.rbegin();
       cur_object_pair != history_objects.rend(); ++cur_object_pair) {
    TrackedObjectPtr cur_object = cur_object_pair->second;
    if (visible_obj_idx >= convergence_score_list.size() ||
        visible_obj_idx >= history_objects.size()) {
      break;
    }
    velocity_residual =
        cur_object->selected_measured_velocity -
        (velocity_source_is_belief ? new_object->belief_velocity
                                   : new_object->output_velocity);
    convergence_score_list[visible_obj_idx] =
        base_convergence_noise /
        std::max(base_convergence_noise, velocity_residual.norm());
    ++visible_obj_idx;
  }
  // Compute convergence confidence
  if (!new_object->converged) {
    double min_convergence_confidence_score = 1.0;  // maximum
    for (size_t i = 0; i < static_cast<size_t>(boostup_need_history_size);
         ++i) {
      if (min_convergence_confidence_score > convergence_score_list[i]) {
        min_convergence_confidence_score = convergence_score_list[i];
      }
    }
    new_object->convergence_confidence =
        static_cast<float>(min_convergence_confidence_score);
  } else {
    double mean_convergence_confidence_score = 0;  // mean
    for (size_t i = 0; i < static_cast<size_t>(boostup_need_history_size);
         ++i) {
      mean_convergence_confidence_score += convergence_score_list[i];
    }
    mean_convergence_confidence_score /= boostup_need_history_size;
    new_object->convergence_confidence =
        static_cast<float>(mean_convergence_confidence_score);
  }
}

ConvergenceEstimationAndBoostUp()

void apollo::perception::radar4d::MrfMotionFilter::ConvergenceEstimationAndBoostUp ( const MrfTrackDataConstPtr &  track_data, const TrackedObjectConstPtr &  latest_object, TrackedObjectPtr  new_object  )

protected

Estimate convergence confidence and boost up state

参数

track_data	history track data
latest_object	latest object in the track data
new_object	new object to be updated

在文件 mrf_motion_filter.cc 第 311 行定义.

                                                                             {
  new_object->boostup_need_history_size =
      latest_object->boostup_need_history_size;
  new_object->converged = latest_object->converged;
  new_object->convergence_confidence = latest_object->convergence_confidence;
 
  // +1 means new object measure velocity
  // -1 means first object without measure velocity not considered
  size_t window_size = 1 + track_data->history_objects_.size() - 1;
 
  // Boostup convergence when its confidence is small than minimum
  ComputeConvergenceConfidence(track_data, new_object, true);
  UpdateConverged(track_data, new_object);
 
  // do not boostup belief if usable measure velocity is not enough
  if (window_size <
      static_cast<size_t>(new_object->boostup_need_history_size)) {
    return;
  }
  // boostup belief if not converged yet
  if (!new_object->converged) {
    BoostupState(track_data, new_object);
    ComputeConvergenceConfidence(track_data, new_object, true);
    UpdateConverged(track_data, new_object);
  }
}

Init()

bool apollo::perception::radar4d::MrfMotionFilter::Init ( const MrfFilterInitOptions &  options = MrfFilterInitOptions() )

overridevirtual

Init mrf fitler

参数

options

返回

true

false

实现了 apollo::perception::radar4d::MrfBaseFilter.

在文件 mrf_motion_filter.cc 第 32 行定义.

                                                              {
  std::string config_file = "mrf_motion_filter.pb.txt";
  if (!options.config_file.empty()) {
    config_file = options.config_file;
  }
  config_file = GetConfigFile(
      options.config_path, config_file);
  MrfMotionFilterConfig config;
  ACHECK(cyber::common::GetProtoFromFile(config_file, &config));
  use_adaptive_ = config.use_adaptive();
  use_breakdown_ = config.use_breakdown();
  use_convergence_boostup_ = config.use_convergence_boostup();
  init_velocity_variance_ = config.init_velocity_variance();
  init_acceleration_variance_ = config.init_acceleration_variance();
  measured_velocity_variance_ = config.measured_velocity_variance();
  predict_variance_per_sqrsec_ = config.predict_variance_per_sqrsec();
  boostup_history_size_minimum_ = config.boostup_history_size_minimum();
  boostup_history_size_maximum_ = config.boostup_history_size_maximum();
  converged_confidence_minimum_ = config.converged_confidence_minimum();
  noise_maximum_ = config.noise_maximum();
  trust_orientation_range_ = config.trust_orientation_range();
 
  motion_measurer_.reset(new MrfMotionMeasurement);
 
  motion_refiner_.reset(new MrfMotionRefiner);
  MrfMotionRefinerInitOptions mrf_motion_refiner_options;
  mrf_motion_refiner_options.config_path = options.config_path;
  ACHECK(motion_refiner_->Init(mrf_motion_refiner_options));
  return true;
}

InitializeTrackState()

void apollo::perception::radar4d::MrfMotionFilter::InitializeTrackState ( TrackedObjectPtr  new_object )

protected

Initialize track state and store in the new object

参数

new_object

new object to be updated

在文件 mrf_motion_filter.cc 第 106 行定义.

                                                                      {
  new_object->boostup_need_history_size =
      static_cast<int>(boostup_history_size_minimum_);
  new_object->convergence_confidence = use_convergence_boostup_ ? 0.0 : 1.0;
  new_object->converged = !use_convergence_boostup_;
  new_object->update_quality = 1.0;
  new_object->selected_measured_velocity = Eigen::Vector3d::Zero();
  new_object->selected_measured_acceleration = Eigen::Vector3d::Zero();
  new_object->belief_anchor_point = new_object->anchor_point;
  new_object->belief_velocity = Eigen::Vector3d::Zero();
  new_object->belief_acceleration = Eigen::Vector3d::Zero();
  new_object->belief_velocity_gain = Eigen::Vector3d::Zero();
  new_object->state = Eigen::Vector4d::Zero();
  new_object->measurement_covariance = Eigen::Matrix4d();
  new_object->state_covariance = Eigen::Matrix4d();
 
  // covariances initialize
  new_object->velocity_covariance =
      Eigen::Matrix3d::Identity() * init_velocity_variance_;
  new_object->belief_velocity_online_covariance =
      new_object->velocity_covariance;
 
  new_object->state_covariance = Eigen::Matrix4d::Identity();
  new_object->state_covariance.block<2, 2>(0, 0) *= init_velocity_variance_;
  new_object->state_covariance.block<2, 2>(2, 2) *= init_acceleration_variance_;
 
  StateToBelief(new_object);
  BeliefToOutput(new_object);
}

KalmanFilterUpdateWithPartialObservation()

void apollo::perception::radar4d::MrfMotionFilter::KalmanFilterUpdateWithPartialObservation ( const MrfTrackDataConstPtr &  track_data, const TrackedObjectConstPtr &  latest_object, TrackedObjectPtr  new_object  )

protected

Update state with kalman filter, constant acceleration motion model, only velocity measurement is observed

参数

track_data	history track data
latest_object	latest object in the track data
new_object	new object to be updated

在文件 mrf_motion_filter.cc 第 136 行定义.

                                                                             {
  double range = new_object->object_ptr->center.norm();
 
  const Eigen::Vector4d& last_state = latest_object->state;
  const Eigen::Matrix4d& last_state_covariance =
      latest_object->state_covariance;
 
  double time_diff = new_object->object_ptr->latest_tracked_time -
                     latest_object->object_ptr->latest_tracked_time;
  if (time_diff < EPSION_TIME) {  // Very small time than assign
    time_diff = DEFAULT_FPS;
  }
  Eigen::Matrix4d transition = Eigen::Matrix4d::Identity();
  transition(0, 2) = transition(1, 3) = time_diff;
 
  // composition with rotation
  if (new_object->type != base::ObjectType::PEDESTRIAN &&
      range < trust_orientation_range_) {
    Eigen::Vector2d cur_dir = new_object->direction.head<2>();
    Eigen::Vector2d pre_dir = latest_object->direction.head<2>();
    cur_dir.normalize();
    pre_dir.normalize();
    double cos_theta = cur_dir.dot(pre_dir);
    double sin_theta = pre_dir(0) * cur_dir(1) - pre_dir(1) * cur_dir(0);
    Eigen::Matrix2d rot;
    rot << cos_theta, -sin_theta, sin_theta, cos_theta;
    Eigen::Matrix4d rot_extend = Eigen::Matrix4d::Zero();
    rot_extend.block<2, 2>(0, 0) = rot;
    rot_extend.block<2, 2>(2, 2) = rot;
    transition = rot_extend * transition;
  }
 
  auto& state = new_object->state;
  auto& state_covariance = new_object->state_covariance;
  auto measurement_covariance =
      new_object->measurement_covariance.block<2, 2>(0, 0);
  // 1. prediction stage
  Eigen::Matrix4d predict_covariance = Eigen::Matrix4d::Identity() *
                                       predict_variance_per_sqrsec_ *
                                       time_diff * time_diff;
  state = transition * last_state;
  state_covariance =
      transition * last_state_covariance * transition.transpose() +
      predict_covariance;
 
  // 2. measurement update stage
  Eigen::Vector2d measurement;
  measurement << new_object->selected_measured_velocity.head<2>();
 
  Eigen::Vector2d direction = new_object->direction.head<2>();
  direction.normalize();
  Eigen::Vector2d odirection(direction(1), -direction(0));
  if (new_object->type == base::ObjectType::PEDESTRIAN &&
      range < trust_orientation_range_) {
    measurement_covariance = Eigen::Matrix2d::Identity();
    measurement_covariance *= measured_velocity_variance_;
  } else {
    const double kVarianceAmplifier = 9.0;
    measurement_covariance =
        measured_velocity_variance_ * direction * direction.transpose() +
        (measured_velocity_variance_ +
         fabs(measurement.dot(odirection)) * kVarianceAmplifier) *
            odirection * odirection.transpose();
  }
 
  Eigen::Matrix<double, 2, 4> observation_transform;
  observation_transform.block<2, 2>(0, 0).setIdentity();
  observation_transform.block<2, 2>(0, 2).setZero();
  Eigen::Matrix<double, 4, 2> kalman_gain_matrix =
      static_cast<Eigen::Matrix<double, 4, 2, 0, 4, 2>>(
          state_covariance * observation_transform.transpose() *
          (observation_transform * state_covariance *
               observation_transform.transpose() +
           measurement_covariance)
              .inverse());
  Eigen::Vector4d state_gain =
      static_cast<Eigen::Matrix<double, 4, 1, 0, 4, 1>>(
          kalman_gain_matrix * (measurement - observation_transform * state));
 
  // 3. gain adjustment and estimate posterior
  StateGainAdjustment(track_data, latest_object, new_object, &state_gain);
 
  state = state + state_gain;
  state_covariance = (Eigen::Matrix4d::Identity() -
                      kalman_gain_matrix * observation_transform) *
                     state_covariance;
 
  // 4. finally, state to belief and output to keep consistency
  new_object->belief_velocity_gain << state_gain.head<2>(), 0;
}

Name()

std::string apollo::perception::radar4d::MrfMotionFilter::Name ( ) const

inlineoverridevirtual

Get class name

返回

std::string

实现了 apollo::perception::radar4d::MrfBaseFilter.

在文件 mrf_motion_filter.h 第 63 行定义.

{ return "MrfMotionFilter"; }

OnlineCovarianceEstimation()

void apollo::perception::radar4d::MrfMotionFilter::OnlineCovarianceEstimation ( const MrfTrackDataConstPtr &  track_data, TrackedObjectPtr  object  )

protected

Estimate covariance considering history measurment

参数

track_data	history track data
object	new object to be updated

在文件 mrf_motion_filter.cc 第 275 行定义.

                                                                     {
  object->belief_velocity_online_covariance = Eigen::Matrix3d::Zero();
  size_t evaluate_window =
      std::min(track_data->history_objects_.size(),
               static_cast<size_t>(object->boostup_need_history_size));
  if (evaluate_window == 0) {
    // a default large covariance
    object->belief_velocity_online_covariance = Eigen::Matrix3d::Identity() *
                                                predict_variance_per_sqrsec_ *
                                                predict_variance_per_sqrsec_;
    return;
  }
  // compute online covariance
  auto cur_obj_pair = track_data->history_objects_.rbegin();
  for (size_t i = 0; i < evaluate_window; ++i) {
    Eigen::Vector3d velocity_resisual =
        cur_obj_pair->second->selected_measured_velocity -
        object->output_velocity;
    if (velocity_resisual.head(2).norm() <
        std::numeric_limits<double>::epsilon()) {
      velocity_resisual =
          Eigen::Vector3d::Identity() * noise_maximum_ * noise_maximum_;
    }
    ++cur_obj_pair;
    object->belief_velocity_online_covariance.block<2, 2>(0, 0) +=
        velocity_resisual.head<2>() * velocity_resisual.head<2>().transpose();
  }
  object->belief_velocity_online_covariance /=
      static_cast<double>(evaluate_window);
  object->belief_velocity_online_covariance +=
      Eigen::Matrix3d::Identity() * noise_maximum_ * noise_maximum_;
  object->output_velocity_uncertainty =
      object->belief_velocity_online_covariance;
}

StateGainAdjustment()

void apollo::perception::radar4d::MrfMotionFilter::StateGainAdjustment ( const MrfTrackDataConstPtr &  track_data, const TrackedObjectConstPtr &  latest_object, const TrackedObjectConstPtr &  new_object, Eigen::Vector4d *  gain  )

protected

Adjust kalman state gain with several strategies

参数

track_data	history track data
latest_object	latest object in the track data
new_object	new object to be updated
gain	state gain

在文件 mrf_motion_filter.cc 第 229 行定义.

                                                                  {
  // 1 quality penalize and gain break down
  if (use_adaptive_) {
    *gain *= new_object->update_quality;
  }
  // 2 breakdown the constrained the max allowed change of state
  if (use_breakdown_) {
    // velocity breakdown threshold
    double velocity_breakdown_threshold = 10.0 - (track_data->age_ - 1) * 2;
    velocity_breakdown_threshold = std::max(velocity_breakdown_threshold, 0.3);
    double velocity_gain = gain->head<2>().norm();
    if (velocity_gain > velocity_breakdown_threshold) {
      gain->head<2>() *= velocity_breakdown_threshold / velocity_gain;
    }
    // acceleration breakdown threshold
    double acceleration_breakdown_threshold = 2.0;
    double acceleration_gain = gain->tail<2>().norm();
    if (acceleration_gain > acceleration_breakdown_threshold) {
      gain->tail<2>() *= acceleration_breakdown_threshold / acceleration_gain;
    }
  }
}

StateToBelief()

void apollo::perception::radar4d::MrfMotionFilter::StateToBelief ( TrackedObjectPtr  object )

protected

Synchronize state to belief to keep consistency

参数

object

new object to be updated

在文件 mrf_motion_filter.cc 第 255 行定义.

                                                           {
  object->belief_velocity << object->state.head<2>(), 0;
  object->belief_acceleration << object->state.tail<2>(), 0;
}

UpdateConverged()

void apollo::perception::radar4d::MrfMotionFilter::UpdateConverged ( const MrfTrackDataConstPtr &  track_data, TrackedObjectPtr  object  )

protected

Update convergence confidence

参数

track_data	history track data
object	new object to be updated

在文件 mrf_motion_filter.cc 第 466 行定义.

                                                               {
  if (object->convergence_confidence > converged_confidence_minimum_) {
    // set converged true
    object->converged = true;
    // increase cached history size if necessary
    if (track_data->history_objects_.size() <
        static_cast<size_t>(object->boostup_need_history_size)) {
      return;
    }
    if (static_cast<size_t>(object->boostup_need_history_size) <
        boostup_history_size_maximum_) {
      object->boostup_need_history_size += 1;
    }
  } else {
    object->converged = false;
  }
}

UpdateWithObject()

void apollo::perception::radar4d::MrfMotionFilter::UpdateWithObject ( const MrfFilterOptions &  options, const MrfTrackDataConstPtr &  track_data, TrackedObjectPtr  new_object  )

overridevirtual

Updating motion filter with object

参数

options	for updating
track_data	track data, not include new object
new_object	for updating

实现了 apollo::perception::radar4d::MrfBaseFilter.

在文件 mrf_motion_filter.cc 第 63 行定义.

                                                                    {
  if (track_data->age_ == 0) {
    InitializeTrackState(new_object);
    return;
  }
  if (new_object->is_background) {
    new_object->output_velocity.setZero();
    new_object->output_velocity_uncertainty = Eigen::Matrix3d::Identity();
    return;
  }
  // 1. compute measurement
  motion_measurer_->ComputeMotionMeasurment(track_data, new_object);
  if (!use_adaptive_) {
    new_object->update_quality = 1.0;
  }
  TrackedObjectConstPtr latest_object = track_data->GetLatestObject().second;
  // 2. kalman filter update
  KalmanFilterUpdateWithPartialObservation(track_data, latest_object,
                                           new_object);
  StateToBelief(new_object);
  // 3. boostup state and static object velocity clapping
  if (use_convergence_boostup_) {
    ConvergenceEstimationAndBoostUp(track_data, latest_object, new_object);
  }
  ClipingState(new_object);
  StateToBelief(new_object);
  // 4. online covariance estimation
  BeliefToOutput(new_object);
  // 5. post refine and recompute online covariance if necessary
  if (motion_refiner_->Refine(track_data, new_object)) {
    // use output velocity to recompute convergence confidence
    ComputeConvergenceConfidence(track_data, new_object, false);
    UpdateConverged(track_data, new_object);
  }
  OnlineCovarianceEstimation(track_data, new_object);
}

UpdateWithoutObject()

void apollo::perception::radar4d::MrfMotionFilter::UpdateWithoutObject ( const MrfFilterOptions &  options, double  timestamp, MrfTrackDataPtr  track_data  )

overridevirtual

Updating motion filter without object

参数

options	for updating
timestamp	current timestamp
track_data	track data to be updated

实现了 apollo::perception::radar4d::MrfBaseFilter.

在文件 mrf_motion_filter.cc 第 102 行定义.

                                                                      {}

类成员变量说明

boostup_history_size_maximum_

size_t apollo::perception::radar4d::MrfMotionFilter::boostup_history_size_maximum_ = 6

protected

在文件 mrf_motion_filter.h 第 188 行定义.

boostup_history_size_minimum_

size_t apollo::perception::radar4d::MrfMotionFilter::boostup_history_size_minimum_ = 3

protected

在文件 mrf_motion_filter.h 第 187 行定义.

converged_confidence_minimum_

double apollo::perception::radar4d::MrfMotionFilter::converged_confidence_minimum_ = 0.5

protected

在文件 mrf_motion_filter.h 第 189 行定义.

DEFAULT_FPS

const double apollo::perception::radar4d::MrfMotionFilter::DEFAULT_FPS = 0.1

protected

在文件 mrf_motion_filter.h 第 172 行定义.

EPSION_TIME

const double apollo::perception::radar4d::MrfMotionFilter::EPSION_TIME = 1e-3

protected

在文件 mrf_motion_filter.h 第 171 行定义.

init_acceleration_variance_

double apollo::perception::radar4d::MrfMotionFilter::init_acceleration_variance_ = 10.0

protected

在文件 mrf_motion_filter.h 第 183 行定义.

init_velocity_variance_

double apollo::perception::radar4d::MrfMotionFilter::init_velocity_variance_ = 5.0

protected

在文件 mrf_motion_filter.h 第 182 行定义.

measured_velocity_variance_

double apollo::perception::radar4d::MrfMotionFilter::measured_velocity_variance_ = 0.4

protected

在文件 mrf_motion_filter.h 第 184 行定义.

motion_measurer_

std::shared_ptr<MrfMotionMeasurement> apollo::perception::radar4d::MrfMotionFilter::motion_measurer_

protected

在文件 mrf_motion_filter.h 第 174 行定义.

motion_refiner_

std::shared_ptr<MrfMotionRefiner> apollo::perception::radar4d::MrfMotionFilter::motion_refiner_

protected

在文件 mrf_motion_filter.h 第 176 行定义.

noise_maximum_

double apollo::perception::radar4d::MrfMotionFilter::noise_maximum_ = 0.1

protected

在文件 mrf_motion_filter.h 第 190 行定义.

predict_variance_per_sqrsec_

double apollo::perception::radar4d::MrfMotionFilter::predict_variance_per_sqrsec_ = 50.0

protected

在文件 mrf_motion_filter.h 第 185 行定义.

trust_orientation_range_

double apollo::perception::radar4d::MrfMotionFilter::trust_orientation_range_ = 40.0

protected

在文件 mrf_motion_filter.h 第 191 行定义.

use_adaptive_

bool apollo::perception::radar4d::MrfMotionFilter::use_adaptive_ = true

protected

在文件 mrf_motion_filter.h 第 178 行定义.

use_breakdown_

bool apollo::perception::radar4d::MrfMotionFilter::use_breakdown_ = true

protected

在文件 mrf_motion_filter.h 第 179 行定义.

use_convergence_boostup_

bool apollo::perception::radar4d::MrfMotionFilter::use_convergence_boostup_ = true

protected

在文件 mrf_motion_filter.h 第 180 行定义.

---

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

• modules/perception/radar4d_detection/lib/tracker/multi_radar_fusion/mrf_motion_filter.h
• modules/perception/radar4d_detection/lib/tracker/multi_radar_fusion/mrf_motion_filter.cc