apollo::perception::camera::Target结构体 参考

#include <target.h>

apollo::perception::camera::Target 的协作图:

Public 成员函数
	Target (const TargetParam &param)
void	Init (const TargetParam &param)
	init Target
void	Add (TrackObjectPtr object)
	add object to tracked_objects
void	RemoveOld (int frame_id)
	remove objects older than frame_id
void	Clear ()
	clear tracked_objects
void	Predict (CameraTrackingFrame *frame)
	using kalman filter to predict the tracked_objects
void	Update (CameraTrackingFrame *frame)
	using kalman filter to correct the tracked_objects todo(zero): update world in bev
void	Update2D (CameraTrackingFrame *frame)
	update 2d
void	Update3D (CameraTrackingFrame *frame)
	update 3d
void	UpdateType (CameraTrackingFrame *frame)
	update type
int	Size () const
	return the size of tracked_objects
TrackObjectPtr	get_object (int index) const
	Get the object accoding to index
TrackObjectPtr	operator[] (int index) const
bool	isTracked () const
	return whether the target is tracked
bool	isLost () const
	return whether the target is lost

Public 属性
int	lost_age = 0
int	id = 0
double	start_ts = 0.0
FirstOrderRCLowPassFilter	direction
TrackObjectPtr	latest_object = nullptr
base::ObjectSubType	type = base::ObjectSubType::MAX_OBJECT_TYPE
KalmanFilterConstVelocity	world_center
MeanFilter	world_center_for_unmovable
KalmanFilterConstState< 2 >	world_center_const
MeanFilter	displacement_theta
MaxNMeanFilter	world_lwh
MeanFilter	world_lwh_for_unmovable
MeanFilter	world_velocity
std::vector< float >	type_probs
TargetParam	target_param_
FirstOrderRCLowPassFilter	image_wh
KalmanFilterConstVelocity	image_center
TrackObjectPtrs	tracked_objects

Protected 属性
ObjectTemplateManager *	object_template_manager_ = nullptr

详细描述

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

构造及析构函数说明

Target()

apollo::perception::camera::Target::Target ( const TargetParam &  param )

explicit

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

{ Init(param); }

成员函数说明

Add()

void apollo::perception::camera::Target::Add

(

TrackObjectPtr

object

)

add object to tracked_objects

参数

object

在文件 target.cc 第 43 行定义.

                                      {
  if (tracked_objects.empty()) {
    start_ts = object->timestamp;
    id = Target::global_track_id++;
    type = object->object->sub_type;
  }
  object->object->track_id = id;
  object->object->tracking_time = object->timestamp - start_ts;
  object->object->latest_tracked_time = object->timestamp;
  latest_object = object;
  lost_age = 0;
  tracked_objects.push_back(object);
}

Clear()

void apollo::perception::camera::Target::Clear

(

)

clear tracked_objects

在文件 target.cc 第 33 行定义.

{ tracked_objects.clear(); }

get_object()

TrackObjectPtr apollo::perception::camera::Target::get_object

(

int

index

)

const

Get the object accoding to index

参数

index

返回

TrackObjectPtr

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

                                                 {
  CHECK_GT(static_cast<int>(tracked_objects.size()), 0);
  CHECK_LT(index, static_cast<int>(tracked_objects.size()));
  CHECK_GE(index, -static_cast<int>(tracked_objects.size()));
  return tracked_objects[(index + tracked_objects.size()) %
                         tracked_objects.size()];
}

Init()

void apollo::perception::camera::Target::Init

(

const TargetParam &

param

)

init Target

参数

param

在文件 target.cc 第 65 行定义.

                                          {
  target_param_ = param;
  id = -1;
  lost_age = 0;
 
  world_lwh.SetWindow(param.world_lhw_history());
  world_lwh_for_unmovable.SetWindow(param.world_lhw_history());
  image_wh.SetAlpha(param.image_wh_update_rate());
  // TODO(gaohan)  should update variance when predict and update
  image_center.variance_ *= target_param_.image_center().init_variance();
  image_center.measure_noise_ *=
      target_param_.image_center().measure_variance();
  image_center.process_noise_ *=
      target_param_.image_center().process_variance();
  world_center.variance_ *= target_param_.world_center().init_variance();
  world_center.process_noise_ *= 1;
  world_center.measure_noise_ *= 1;
 
  type_probs.assign(static_cast<int>(base::ObjectSubType::MAX_OBJECT_TYPE),
                    0.0f);
  world_center_for_unmovable.SetWindow(param.world_lhw_history());
  world_velocity.SetWindow(param.mean_filter_window());
 
  // record history state after kalman filter
  history_world_states_.set_capacity(param.world_state_history());
 
  // record orientation of displacement
  displacement_theta.SetWindow(param.mean_filter_window());
  direction.SetAlpha(param.direction_filter_ratio());
  // Init constant position Kalman Filter
  world_center_const.covariance_.setIdentity();
  world_center_const.measure_noise_.setIdentity();
  world_center_const.process_noise_.setIdentity();
  world_center_const.covariance_ *=
      target_param_.world_center().init_variance();
  // Init object template
  object_template_manager_ = ObjectTemplateManager::Instance();
}

isLost()

bool apollo::perception::camera::Target::isLost

(

)

const

return whether the target is lost

返回

true

false

在文件 target.cc 第 516 行定义.

{ return lost_age > 0; }

isTracked()

bool apollo::perception::camera::Target::isTracked

(

)

const

return whether the target is tracked

返回

true

false

在文件 target.cc 第 513 行定义.

                             {
  return Size() >= target_param_.tracked_life();
}

operator[]()

TrackObjectPtr apollo::perception::camera::Target::operator[]

(

int

index

)

const

在文件 target.cc 第 35 行定义.

{ return get_object(index); }

Predict()

void apollo::perception::camera::Target::Predict

(

CameraTrackingFrame *

frame

)

using kalman filter to predict the tracked_objects

参数

frame

在文件 target.cc 第 105 行定义.

                                               {
  auto delta_t =
      static_cast<float>(frame->timestamp - latest_object->timestamp);
  if (delta_t < 0) {
    return;
  }
  image_center.Predict(delta_t);
  float acc_variance = target_param_.world_center().process_variance();
  float delta_t_2 = delta_t * delta_t;
  float pos_variance = 0.25f * acc_variance * delta_t_2 * delta_t_2;
  float vel_variance = acc_variance * delta_t_2;
  world_center.process_noise_(0, 0) = pos_variance;
  world_center.process_noise_(1, 1) = pos_variance;
  world_center.process_noise_(2, 2) = vel_variance;
  world_center.process_noise_(3, 3) = vel_variance;
  world_center.Predict(delta_t);
 
  // const position kalman predict
  world_center_const.process_noise_.setIdentity();
  world_center_const.process_noise_(0, 0) = vel_variance * delta_t_2;
  world_center_const.process_noise_(1, 1) =
      world_center_const.process_noise_(0, 0);
  world_center_const.Predict(delta_t);
}

RemoveOld()

void apollo::perception::camera::Target::RemoveOld

(

int

frame_id

)

remove objects older than frame_id

参数

frame_id

在文件 target.cc 第 56 行定义.

                                   {
  size_t index = 0;
  while (index < tracked_objects.size() &&
         tracked_objects[index]->indicator.frame_id < frame_id) {
    ++index;
  }
  tracked_objects.erase(tracked_objects.begin(),
                        tracked_objects.begin() + index);
}

Size()

int apollo::perception::camera::Target::Size

(

)

const

return the size of tracked_objects

返回

int

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

{ return static_cast<int>(tracked_objects.size()); }

Update()

void apollo::perception::camera::Target::Update

(

CameraTrackingFrame *

frame

)

using kalman filter to correct the tracked_objects todo(zero): update world in bev

参数

frame

在文件 target.cc 第 309 行定义.

                                              {
  auto object = latest_object->object;
  if (!isLost()) {
    // todo(zero): need to fix alpha
    float alpha = 0.01f;
 
    Eigen::Vector4d size_measurement;
    size_measurement << alpha, object->size(0), object->size(1),
        object->size(2);
    world_lwh.AddMeasure(size_measurement);
    world_lwh_for_unmovable.AddMeasure(size_measurement);
    // update 3d object size
    if (object->type == base::ObjectType::UNKNOWN_UNMOVABLE) {
      object->size =
          world_lwh_for_unmovable.get_state().block<3, 1>(1, 0).cast<float>();
    } else {
      object->size = world_lwh.get_state().block<3, 1>(1, 0).cast<float>();
    }
    ADEBUG << " size is " << world_lwh.get_state().transpose();
  }
 
  Update3D(frame);
}

Update2D()

void apollo::perception::camera::Target::Update2D

(

CameraTrackingFrame *

frame

)

update 2d

参数

frame

在文件 target.cc 第 130 行定义.

                                                {
  // measurements
  auto obj = latest_object->object;
  float width = static_cast<float>(frame->data_provider->src_width());
  float height = static_cast<float>(frame->data_provider->src_height());
  base::RectF rect(latest_object->projected_box);
  base::Point2DF center = rect.Center();
 
  if (!isLost()) {
    Eigen::Vector2d measurement;
    measurement << rect.width, rect.height;
    image_wh.AddMeasure(measurement);
    measurement << center.x, center.y;
    image_center.Correct(measurement);
 
    // update 2d bbox size
    Eigen::Vector4d state;
    state = image_center.get_state();
    center.x = static_cast<float>(state(0));
    center.y = static_cast<float>(state(1));
    ADEBUG << "2d move:" << id << " " << state(2) << " " << state(3);
 
    auto shape = image_wh.get_state();
    rect.width = static_cast<float>(shape(0));
    rect.height = static_cast<float>(shape(1));
    rect.SetCenter(center);
    RefineBox(rect, width, height, &rect);
    latest_object->projected_box = rect;
  }
}

Update3D()

void apollo::perception::camera::Target::Update3D

(

CameraTrackingFrame *

frame

)

update 3d

参数

frame

在文件 target.cc 第 161 行定义.

                                                {
  auto object = latest_object->object;
  if (!isLost()) {
    // camera coordinate
    Eigen::Vector2d z;
    z << std::sin(object->theta), std::cos(object->theta);
    direction.AddMeasure(z);
    z = direction.get_state();
    float theta = static_cast<float>(std::atan2(z[0], z[1]));
    AINFO << "dir " << id << " " << object->theta << " " << theta;
    object->theta = theta;
    object->direction[0] = static_cast<float>(cos(object->theta));
    object->direction[1] = static_cast<float>(sin(object->theta));
    object->direction[2] = 0;
 
    z << object->center(0), object->center(1);
    if (object->type == base::ObjectType::UNKNOWN_UNMOVABLE) {
      world_center_for_unmovable.AddMeasure(z);
    } else {
      // Eigen::Vector4d x = world_center.get_state();
      float obj_2_car_x = object->camera_supplement.local_center[0];
      float obj_2_car_z = object->camera_supplement.local_center[2];
      float obj_distance_to_main_car =
          obj_2_car_x * obj_2_car_x + obj_2_car_z * obj_2_car_z;
      float dis_err = target_param_.world_center().measure_variance() *
                      obj_distance_to_main_car;
      world_center.measure_noise_.setIdentity();
      world_center.measure_noise_ *= dis_err;
      world_center.Correct(z);
      ADEBUG << "Velocity: " << id << " " << z.transpose() << " "
             << world_center.get_state().transpose();
 
      // const position kalman correct
      world_center_const.measure_noise_.setIdentity();
      world_center_const.measure_noise_ *= dis_err;
      KalmanFilterConstState<2>::VectorNd pos_measure;
      pos_measure << object->center(0), object->center(1);
      world_center_const.Correct(z);
    }
  }
 
  if (object->type == base::ObjectType::UNKNOWN_UNMOVABLE) {
    const Eigen::VectorXd &x = world_center_for_unmovable.get_state();
    const Eigen::MatrixXd &var = world_center_for_unmovable.get_variance();
    object->center(0) = x(0);
    object->center(1) = x(1);
    object->center_uncertainty(0) = static_cast<float>(var(0));
    object->center_uncertainty(1) = static_cast<float>(var(1));
    object->velocity(0) = 0;
    object->velocity(1) = 0;
    object->velocity(2) = 0;
  } else {
    Eigen::Vector4d x = world_center.get_state();
    // TODO(gaohan02): refactor
    if (tracked_objects.size() > 10) {
      auto pose1 = get_object(-2)->object->center;
      auto pose2 = get_object(-10)->object->center;
      double ts1 = get_object(-2)->timestamp;
      double ts2 = get_object(-10)->timestamp;
      Eigen::Vector3d vel = (pose1 - pose2) / (ts1 - ts2);
      double speed1 = std::sqrt(vel(0) * vel(0) + vel(1) * vel(1));
      double speed2 = std::sqrt(x(2) * x(2) + x(3) * x(3));
      double ratio = (Equal(speed1, 0)) ? 0 : speed2 / speed1;
      ADEBUG << "Target: " << id << " " << vel.transpose() << " , "
             << x.block<2, 1>(2, 0).transpose();
      if (ratio > target_param_.too_large_velocity_ratio()) {
        world_center.MagicVelocity(vel);
        ADEBUG << "Velocity too large";
      } else if (ratio > target_param_.large_velocity_ratio()) {
        vel(0) = (x(2) + vel(0)) / 2;
        vel(1) = (x(3) + vel(1)) / 2;
        world_center.MagicVelocity(vel);
        ADEBUG << "Velocity large";
      } else {
        ADEBUG << "Velocity normal";
      }
    }
 
    x = world_center.get_state();
    double speed = std::sqrt(x(2) * x(2) + x(3) * x(3));
    const std::map<base::ObjectSubType, float> &kTypeSpeedLimit =
        object_template_manager_->TypeSpeedLimit();
    if (speed > kTypeSpeedLimit.at(object->sub_type)) {
      double ratio = kTypeSpeedLimit.at(object->sub_type) / speed;
      Eigen::Vector2d vel;
      vel << x(2) * ratio, x(3) * ratio;
      world_center.MagicVelocity(vel);
      x = world_center.get_state();
    }
    object->center(0) = x(0);
    object->center(1) = x(1);
    object->center_uncertainty.setConstant(0);
    object->center_uncertainty(0, 0) =
        static_cast<float>(world_center.variance_(0, 0));
    object->center_uncertainty(1, 1) =
        static_cast<float>(world_center.variance_(1, 1));
    object->velocity(0) = static_cast<float>(x(2));
    object->velocity(1) = static_cast<float>(x(3));
    object->velocity(2) = 0;
    world_velocity.AddMeasure(object->velocity.cast<double>());
    object->velocity_uncertainty = world_velocity.get_variance().cast<float>();
    if (speed > target_param_.velocity_threshold()) {
      object->direction(0) = static_cast<float>(x(2) / speed);
      object->direction(1) = static_cast<float>(x(3) / speed);
      object->direction(2) = 0;
      object->theta = static_cast<float>(std::atan2(x(3), x(2)));
    }
  }
 
  if (object->type != base::ObjectType::UNKNOWN_UNMOVABLE &&
      target_param_.clapping_velocity()) {
    // check displacement orientation
    bool stable_moving = false;
    double avg_vel_norm = world_velocity.get_state().norm();
    if (avg_vel_norm > target_param_.stable_moving_speed()) {
      stable_moving = true;
    } else if (tracked_objects.size() > 1) {
      auto prev_pos = get_object(-2)->object->center;
      auto displacement = object->center - prev_pos;
      Eigen::VectorXd measured_theta(1);
      measured_theta << std::atan2(displacement[1], displacement[0]);
      displacement_theta.AddMeasure(measured_theta);
      // if an object is moving, its displacement orientation should be
      // consistent
      stable_moving = displacement_theta.get_variance()(0, 0) <
                      target_param_.displacement_theta_var();
    }
 
    // check const position kalman residuals
    const auto &residual = world_center_const.residual_;
    if (!stable_moving &&
        residual(0) * residual(0) <
            4 * world_center_const.measure_noise_(0, 0) &&
        residual(1) * residual(1) <
            4 * world_center_const.measure_noise_(1, 1)) {
      object->velocity(0) = 0;
      object->velocity(1) = 0;
    }
 
    // check history states
    history_world_states_.push_back(object);
    ClappingTrackVelocity(object);
  }
 
  // debug velocity
  ADEBUG << "obj_speed--id: " << id << " " << object->velocity.head(2).norm();
}

UpdateType()

void apollo::perception::camera::Target::UpdateType

(

CameraTrackingFrame *

frame

)

update type

参数

frame

在文件 target.cc 第 333 行定义.

                                                  {
  auto object = latest_object->object;
  if (!isLost()) {
    base::RectF rect(object->camera_supplement.box);
    // 1.  6mm: reliable z is 40. intrinsic is approximate 2000
    // 2. 12mm: reliable z is 80. intrinsic is approximate 4000
    // hf = H * f/z = H * 50
    const TemplateMap &kMidTemplateHWL =
        object_template_manager_->MidTemplateHWL();
    float alpha = gaussian(
        rect.height /
            (50 * (kMidTemplateHWL.at(object->sub_type).at(0) + 0.01f)),
        1.0f, target_param_.type_filter_var());
    alpha = std::max(0.01f, alpha);
 
    type_probs[static_cast<int>(object->sub_type)] += alpha;
    auto max_prob = std::max_element(type_probs.begin(), type_probs.end());
    auto index = static_cast<int>(std::distance(type_probs.begin(), max_prob));
    type = static_cast<base::ObjectSubType>(index);
    ADEBUG << "Target " << id << " change type from "
           << static_cast<int>(object->sub_type) << " to "
           << static_cast<int>(type);
    object->sub_type = type;
 
    Eigen::Vector4d size_measurement;
    size_measurement << alpha, object->size(0), object->size(1),
        object->size(2);
    world_lwh.AddMeasure(size_measurement);
    world_lwh_for_unmovable.AddMeasure(size_measurement);
    // update 3d object size
    if (object->type == base::ObjectType::UNKNOWN_UNMOVABLE) {
      object->size =
          world_lwh_for_unmovable.get_state().block<3, 1>(1, 0).cast<float>();
    } else {
      object->size = world_lwh.get_state().block<3, 1>(1, 0).cast<float>();
    }
    ADEBUG << " size is " << world_lwh.get_state().transpose();
  }
}

类成员变量说明

direction

FirstOrderRCLowPassFilter apollo::perception::camera::Target::direction

在文件 target.h 第 135 行定义.

displacement_theta

MeanFilter apollo::perception::camera::Target::displacement_theta

在文件 target.h 第 145 行定义.

id

int apollo::perception::camera::Target::id = 0

在文件 target.h 第 133 行定义.

image_center

KalmanFilterConstVelocity apollo::perception::camera::Target::image_center

在文件 target.h 第 154 行定义.

image_wh

FirstOrderRCLowPassFilter apollo::perception::camera::Target::image_wh

在文件 target.h 第 153 行定义.

latest_object

TrackObjectPtr apollo::perception::camera::Target::latest_object = nullptr

在文件 target.h 第 136 行定义.

lost_age

int apollo::perception::camera::Target::lost_age = 0

在文件 target.h 第 132 行定义.

object_template_manager_

ObjectTemplateManager* apollo::perception::camera::Target::object_template_manager_ = nullptr

protected

在文件 target.h 第 167 行定义.

start_ts

double apollo::perception::camera::Target::start_ts = 0.0

在文件 target.h 第 134 行定义.

target_param_

TargetParam apollo::perception::camera::Target::target_param_

在文件 target.h 第 151 行定义.

tracked_objects

TrackObjectPtrs apollo::perception::camera::Target::tracked_objects

在文件 target.h 第 156 行定义.

type

base::ObjectSubType apollo::perception::camera::Target::type = base::ObjectSubType::MAX_OBJECT_TYPE

在文件 target.h 第 137 行定义.

type_probs

std::vector<float> apollo::perception::camera::Target::type_probs

在文件 target.h 第 150 行定义.

world_center

KalmanFilterConstVelocity apollo::perception::camera::Target::world_center

在文件 target.h 第 138 行定义.

world_center_const

KalmanFilterConstState<2> apollo::perception::camera::Target::world_center_const

在文件 target.h 第 142 行定义.

world_center_for_unmovable

MeanFilter apollo::perception::camera::Target::world_center_for_unmovable

在文件 target.h 第 139 行定义.

world_lwh

MaxNMeanFilter apollo::perception::camera::Target::world_lwh

在文件 target.h 第 147 行定义.

world_lwh_for_unmovable

MeanFilter apollo::perception::camera::Target::world_lwh_for_unmovable

在文件 target.h 第 148 行定义.

world_velocity

MeanFilter apollo::perception::camera::Target::world_velocity

在文件 target.h 第 149 行定义.

---

该结构体的文档由以下文件生成:

• modules/perception/camera_tracking/base/target.h
• modules/perception/camera_tracking/base/target.cc