apollo::perception::camera::Cipv类 参考

#include <cipv_camera.h>

类 apollo::perception::camera::Cipv 继承关系图:

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

Public 成员函数
	Cipv ()=default
	~Cipv ()=default
bool	Init (const Eigen::Matrix3d &homography_im2car, const CipvInitOptions &options=CipvInitOptions()) override
bool	Process (CameraFrame *frame, const CipvOptions &options, const Eigen::Affine3d &world2camera, const base::MotionBufferPtr &motion_buffer) override
std::string	Name () const override
bool	DetermineCipv (const std::vector< base::LaneLine > &lane_objects, const CipvOptions &options, const Eigen::Affine3d &world2camera, std::vector< std::shared_ptr< base::Object > > *objects)
bool	CollectDrops (const base::MotionBufferPtr &motion_buffer, const Eigen::Affine3d &world2camera, std::vector< std::shared_ptr< base::Object > > *objects)
Public 成员函数 继承自 apollo::perception::camera::BaseCipv
	BaseCipv ()=default
virtual	~BaseCipv ()=default

静态 Public 成员函数
static float	VehicleDynamics (const uint32_t tick, const float yaw_rate, const float velocity, const float time_unit, float *x, float *y)
static float	VehicleDynamics (const uint32_t tick, const float yaw_rate, const float velocity, const float time_unit, const float vehicle_half_width, float *center_x, float *ceneter_y, float *left_x, float *left_y, float *right_x, float *right_y)
static bool	MakeVirtualEgoLaneFromYawRate (const float yaw_rate, const float velocity, const float offset_distance, LaneLineSimple *left_lane_line, LaneLineSimple *right_lane_line)

详细描述

在文件 cipv_camera.h 第 52 行定义.

构造及析构函数说明

Cipv()

apollo::perception::camera::Cipv::Cipv ( )

default

~Cipv()

apollo::perception::camera::Cipv::~Cipv ( )

default

成员函数说明

CollectDrops()

bool apollo::perception::camera::Cipv::CollectDrops	(	const base::MotionBufferPtr &	motion_buffer,
		const Eigen::Affine3d &	world2camera,
		std::vector< std::shared_ptr< base::Object > > *	objects
	)

在文件 cipv_camera.cc 第 959 行定义.

                                                                       {
  int motion_size = static_cast<int>(motion_buffer->size());
  if (debug_level_ >= 2) {
    AINFO << " motion_size: " << motion_size;
  }
  if (motion_size <= 0) {
    ADEBUG << " motion_size: " << motion_size;
    return false;
  }
  // std::map<int, std::vector<std::pair<float, float>>>
  //     tmp_object_trackjectories;
  // std::swap(object_trackjectories_, tmp_object_trackjectories);
 
  if (debug_level_ >= 2) {
    AINFO << "object_trackjectories_.size(): " << object_trackjectories_.size();
  }
  for (auto obj : *objects) {
    int cur_id = obj->track_id;
    if (debug_level_ >= 2) {
      AINFO << "target ID: " << cur_id;
    }
    // for (auto point : tmp_object_trackjectories[cur_id]) {
    //   object_trackjectories_[cur_id].emplace_back(point);
    // }
 
    // If it is the first object, set capacity.
    if (object_trackjectories_[cur_id].empty()) {
      object_trackjectories_[cur_id].set_capacity(kDropsHistorySize);
    }
 
    object_id_skip_count_[cur_id] = 0;
 
    auto pos = world2camera * obj->center;
    float center_x = static_cast<float>(pos(2));
    float center_y = static_cast<float>(-pos(0));
 
    object_trackjectories_[cur_id].push_back(
        std::make_pair(center_x, center_y));
 
    if (debug_level_ >= 2) {
      AINFO << "object_trackjectories_[" << cur_id
            << " ].size(): " << object_trackjectories_[cur_id].size();
    }
 
    Eigen::Matrix4f accum_motion_buffer =
        motion_buffer->at(motion_size - 1).motion;
    // Add drops
    for (std::size_t it = object_trackjectories_[cur_id].size() - 1, count = 0;
         it > 0; it--) {
      if (count >= kDropsHistorySize || count > motion_buffer->size()) {
        break;
      }
      if (static_cast<int>(it) + 1 > motion_size) {
        continue;
      }
      Eigen::VectorXf pt =
          Eigen::VectorXf::Zero((*motion_buffer)[0].motion.cols());
      pt(0) = object_trackjectories_[cur_id][it].first;
      pt(1) = object_trackjectories_[cur_id][it].second;
      pt(2) = 0.0f;
      pt(3) = 1.0f;
 
      Eigen::Vector3d transformed_pt;
      accum_motion_buffer *= motion_buffer->at(motion_size - it - 1).motion;
      TranformPoint(pt, accum_motion_buffer, &transformed_pt);
      // TranformPoint(pt, (*motion_buffer)[motion_size - count - 1].motion,
      //               &transformed_pt);
      if (debug_level_ >= 3) {
        AINFO << "(*motion_buffer)[" << motion_size - it - 1 << "].motion:";
        AINFO << motion_buffer->at(motion_size - it - 1).motion;
        AINFO << "accum_motion_buffer[" << motion_size - it - 1 << "] =";
        AINFO << accum_motion_buffer;
        AINFO << "target[" << obj->track_id << "][" << it << "]: ("
              << transformed_pt(0) << ", " << transformed_pt(1) << ")";
      }
      obj->drops[count] = transformed_pt;
      obj->drop_num = count++;
    }
  }
 
  // Currently remove trajectory if they do not exist in the current frame
  // TODO(techoe): need to wait several frames
  for (const auto &each_object : object_trackjectories_) {
    int obj_id = each_object.first;
    bool b_found_id = false;
    for (auto obj : *objects) {
      int cur_id = obj->track_id;
      if (obj_id == cur_id) {
        b_found_id = true;
        break;
      }
    }
    // If object ID was not found erase it from map
    if (!b_found_id && object_trackjectories_[obj_id].size() > 0) {
      //      object_id_skip_count_[obj_id].second++;
      object_id_skip_count_[obj_id]++;
      if (debug_level_ >= 2) {
        AINFO << "object_id_skip_count_[" << obj_id
              << " ]: " << object_id_skip_count_[obj_id];
      }
      if (object_id_skip_count_[obj_id] >= kMaxAllowedSkipObject) {
        if (debug_level_ >= 2) {
          AINFO << "Removed obsolete object " << obj_id;
        }
        object_trackjectories_.erase(obj_id);
        object_id_skip_count_.erase(obj_id);
      }
    }
  }
  if (debug_level_ >= 2) {
    for (auto obj : *objects) {
      int cur_id = obj->track_id;
      AINFO << "obj->track_id: " << cur_id;
      AINFO << "obj->drop_num: " << obj->drop_num;
    }
  }
  return true;
}

DetermineCipv()

bool apollo::perception::camera::Cipv::DetermineCipv	(	const std::vector< base::LaneLine > &	lane_objects,
		const CipvOptions &	options,
		const Eigen::Affine3d &	world2camera,
		std::vector< std::shared_ptr< base::Object > > *	objects
	)

在文件 cipv_camera.cc 第 853 行定义.

                                                                        {
  if (debug_level_ >= 3) {
    AINFO << "Cipv Got SensorObjects with size of " << objects->size();
    AINFO << "Cipv Got lane object with size of " << lane_objects.size();
  }
 
  // float yaw_rate = options.yaw_rate;
  // float velocity = options.velocity;
  int32_t cipv_index = -1;
  //    int32_t old_cipv_track_id = sensor_objects.cipv_track_id;
  int32_t cipv_track_id = -1;
  bool b_left_valid = false;
  bool b_right_valid = false;
  int32_t old_cipv_index = -1;
  EgoLane egolane_image;
  EgoLane egolane_ground;
  EgoLane virtual_egolane_ground;
  egolane_ground.left_line.line_point.clear();
  egolane_ground.right_line.line_point.clear();
  virtual_egolane_ground.left_line.line_point.clear();
  virtual_egolane_ground.right_line.line_point.clear();
 
  // Get ego lanes (in both image and ground coordinate)
  GetEgoLane(lane_objects, &egolane_image, &egolane_ground, &b_left_valid,
             &b_right_valid);
  ElongateEgoLane(lane_objects, b_left_valid, b_right_valid, options.yaw_rate,
                  options.velocity, &egolane_image, &egolane_ground);
 
  CreateVirtualEgoLane(options.yaw_rate, options.velocity,
                       &virtual_egolane_ground);
 
  float min_distance = std::numeric_limits<float>::max();
  float distance;
  for (int32_t i = 0; i < static_cast<int32_t>(objects->size()); ++i) {
    if (debug_level_ >= 2) {
      AINFO << "objects[" << i << "]->track_id: " << (*objects)[i]->track_id;
    }
    if (IsObjectInTheLane((*objects)[i], egolane_image, egolane_ground,
                          world2camera, false, &distance) ||
        IsObjectInTheLane((*objects)[i], egolane_image, virtual_egolane_ground,
                          world2camera, true, &distance)) {
      if (cipv_index < 0 || distance < min_distance) {
        cipv_index = i;
        cipv_track_id = (*objects)[i]->track_id;
        min_distance = distance;
      }
 
      if (debug_level_ >= 2) {
        AINFO << "current cipv_index: " << cipv_index;
      }
    }
    if ((*objects)[i]->track_id == old_cipv_track_id_) {
      old_cipv_index = cipv_index;
    }
  }
  if (debug_level_ >= 1) {
    AINFO << "old_cipv_index: " << old_cipv_index;
    AINFO << "old_cipv_track_id_: " << old_cipv_track_id_;
  }
  if (cipv_index >= 0) {
    if (old_cipv_index >= 0 && old_cipv_index != cipv_index &&
        old_cipv_index < static_cast<int32_t>(objects->size())) {
      (*objects)[old_cipv_index]->b_cipv = false;
    }
    (*objects)[cipv_index]->b_cipv = true;
    old_cipv_track_id_ = cipv_track_id;
    // sensor_objects.cipv_index = cipv_index;
    // sensor_objects.cipv_track_id = cipv_track_id;
    if (debug_level_ >= 1) {
      AINFO << "final cipv_index: " << cipv_index;
      AINFO << "final cipv_track_id: " << cipv_track_id;
      // AINFO << "CIPV Index is changed from " << old_cipv_index << "th
      // object to "
      //            << cipv_index << "th object.";
      // AINFO << "CIPV Track_ID is changed from " << old_cipv_track_id <<
      // " to "
      //            << cipv_track_id << ".";
    }
  } else {
    if (debug_level_ >= 1) {
      AINFO << "No cipv";
    }
  }
 
  return true;
}

Init()

bool apollo::perception::camera::Cipv::Init ( const Eigen::Matrix3d &  homography_im2car, const CipvInitOptions &  options = CipvInitOptions()  )

overridevirtual

实现了 apollo::perception::camera::BaseCipv.

在文件 cipv_camera.cc 第 30 行定义.

                                                {
  b_image_based_cipv_ = options.image_based_cipv;
  debug_level_ =
      options.debug_level;
                    // 0: no debug message
                    // 1: minimal output
                    // 2: some important output
                    // 3: verbose message
                    // 4: visualization
                    // 5: all
                    // -x: specific debugging, where x is the specific number
  time_unit_ = options.average_frame_rate;
  homography_im2car_ = homography_im2car;
  homography_car2im_ = homography_im2car.inverse();
 
  min_laneline_length_for_cipv_ = options.min_laneline_length_for_cipv;
  average_lane_width_in_meter_ = options.average_lane_width_in_meter;
  max_vehicle_width_in_meter_ = options.max_vehicle_width_in_meter;
  margin_vehicle_to_lane_ =
      (average_lane_width_in_meter_ - max_vehicle_width_in_meter_) * 0.5f;
  single_virtual_egolane_width_in_meter_ = max_vehicle_width_in_meter_;
  half_vehicle_width_in_meter_ = max_vehicle_width_in_meter_ * 0.5f;
  half_virtual_egolane_width_in_meter_ =
      single_virtual_egolane_width_in_meter_ * 0.5f;
  old_cipv_track_id_ = -2;
 
  return true;
}

MakeVirtualEgoLaneFromYawRate()

bool apollo::perception::camera::Cipv::MakeVirtualEgoLaneFromYawRate ( const float  yaw_rate, const float  velocity, const float  offset_distance, LaneLineSimple *  left_lane_line, LaneLineSimple *  right_lane_line  )

static

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

                                                                          {
  float center_x = 0.0f;
  float center_y = 0.0f;
  float left_x = 0.0f;
  float left_y = 0.0f;
  float right_x = 0.0f;
  float right_y = 0.0f;
  left_lane_line->line_point.clear();
  right_lane_line->line_point.clear();
 
  for (uint32_t i = 1; i < kMaxNumVirtualLanePoint; ++i) {
    VehicleDynamics(i, yaw_rate, velocity, kAverageFrameRate, offset_distance,
                    &center_x, &center_y, &left_x, &left_y, &right_x, &right_y);
    Point2Df left_point(left_x, left_y);
    left_lane_line->line_point.emplace_back(left_point);
    Point2Df right_point(right_x, right_y);
    right_lane_line->line_point.emplace_back(right_point);
  }
  return true;
}

Name()

std::string apollo::perception::camera::Cipv::Name ( ) const

overridevirtual

实现了 apollo::perception::camera::BaseCipv.

在文件 cipv_camera.cc 第 1116 行定义.

{ return "Cipv"; }

Process()

bool apollo::perception::camera::Cipv::Process ( CameraFrame *  frame, const CipvOptions &  options, const Eigen::Affine3d &  world2camera, const base::MotionBufferPtr &  motion_buffer  )

overridevirtual

实现了 apollo::perception::camera::BaseCipv.

在文件 cipv_camera.cc 第 60 行定义.

                                                         {
  DetermineCipv(frame->lane_objects, options, world2camera,
                          &frame->tracked_objects);
 
  // Get Drop points
  if (motion_buffer->size() > 0) {
  CollectDrops(motion_buffer, world2camera, &frame->tracked_objects);
  } else {
    AWARN << "motion_buffer is empty";
  }
  return true;
}

VehicleDynamics() [1/2]

float apollo::perception::camera::Cipv::VehicleDynamics ( const uint32_t  tick, const float  yaw_rate, const float  velocity, const float  time_unit, const float  vehicle_half_width, float *  center_x, float *  ceneter_y, float *  left_x, float *  left_y, float *  right_x, float *  right_y  )

static

在文件 cipv_camera.cc 第 196 行定义.

                                                            {
  // Option 1. Straight model;
  // *x = time_unit * velocity * static_cast<float>(tick);
  // *y = 0.0f;
 
  // Option 2. Sphere model
  float adjusted_velocity = std::max(kMinVelocity, velocity);
  float theta = static_cast<float>(tick) * time_unit * yaw_rate;
  float displacement = static_cast<float>(tick) * time_unit * adjusted_velocity;
  float offset = half_vehicle_width;
  *center_x = displacement * static_cast<float>(cos(theta));
  *center_y = displacement * static_cast<float>(sin(theta));
  if (theta < 0) {
    offset = -half_vehicle_width;
  }
  *left_x = (displacement + offset) * static_cast<float>(cos(theta)) - offset;
  *left_y = (displacement + offset) * static_cast<float>(sin(theta)) +
            half_vehicle_width;
  *right_x = (displacement - offset) * static_cast<float>(cos(theta)) + offset;
  *right_y = (displacement - offset) * static_cast<float>(sin(theta)) -
             half_vehicle_width;
 
  // Option 3. Bicycle model
  // TODO(techoe): Apply bicycle model for vehicle dynamics (need wheel base)
 
  return true;
}

VehicleDynamics() [2/2]

float apollo::perception::camera::Cipv::VehicleDynamics ( const uint32_t  tick, const float  yaw_rate, const float  velocity, const float  time_unit, float *  x, float *  y  )

static

在文件 cipv_camera.cc 第 175 行定义.

                                                {
  // Option 1. Straight model;
  // *x = time_unit * velocity * static_cast<float>(tick);
  // *y = 0.0f;
 
  // Option 2. Sphere model
  float adjusted_velocity = std::max(kMinVelocity, velocity);
  float theta = static_cast<float>(tick) * time_unit * yaw_rate;
  float displacement = static_cast<float>(tick) * time_unit * adjusted_velocity;
  *x = displacement * static_cast<float>(cos(theta));
  *y = displacement * static_cast<float>(sin(theta));
 
  // Option 3. Bicycle model
  // TODO(techoe): Apply bicycle model for vehicle dynamics (need wheel base)
 
  return true;
}

---

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

• modules/perception/common/camera/common/cipv_camera.h
• modules/perception/common/camera/common/cipv_camera.cc