Apollo 10.0
自动驾驶开放平台
Public 成员函数 | Protected 属性 | 所有成员列表
apollo::perception::camera::ObstacleReference类 参考

#include <obstacle_reference.h>

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

Public 成员函数

EIGEN_MAKE_ALIGNED_OPERATOR_NEW void Init (const ReferenceParam &ref_param, float width, float height)
 
void UpdateReference (const CameraTrackingFrame *frame, const apollo::common::EigenVector< Target > &targets)
 
void CorrectSize (CameraTrackingFrame *frame)
 

Protected 属性

ObjectTemplateManager * object_template_manager_ = nullptr
 

详细描述

在文件 obstacle_reference.h 第 39 行定义.

成员函数说明

◆ CorrectSize()

void apollo::perception::camera::ObstacleReference::CorrectSize ( CameraTrackingFrame *  frame)

在文件 obstacle_reference.cc 第 146 行定义.

146 {
147 const TemplateMap &kMinTemplateHWL =
148 object_template_manager_->MinTemplateHWL();
149 const TemplateMap &kMaxTemplateHWL =
150 object_template_manager_->MaxTemplateHWL();
151 const std::vector<TemplateMap> &kTemplateHWL =
152 object_template_manager_->TemplateHWL();
153
154 for (auto &obj : frame->detected_objects) {
155 float volume_object = obj->size[0] * obj->size[1] * obj->size[2];
156 ADEBUG << "Det " << frame->frame_id << " (" << obj->id << ") "
157 << "ori size:" << obj->size.transpose() << " "
158 << "type: " << static_cast<int>(obj->sub_type) << " "
159 << volume_object << " " << frame->data_provider->sensor_name();
160 base::CameraObjectSupplement &supplement = obj->camera_supplement;
161
162 if (Contain(object_template_manager_->TypeRefinedByTemplate(),
163 obj->sub_type)) {
164 float min_template_volume = 0.0f;
165 float max_template_volume = 0.0f;
166 auto min_tmplt = kMinTemplateHWL.at(obj->sub_type);
167 auto max_tmplt = kMaxTemplateHWL.at(obj->sub_type);
168 min_template_volume = min_tmplt[0] * min_tmplt[1] * min_tmplt[2];
169 max_template_volume = max_tmplt[0] * max_tmplt[1] * max_tmplt[2];
170 if (volume_object < min_template_volume ||
171 obj->size[2] < (1 - ref_param_.height_diff_ratio()) * min_tmplt[0]) {
172 obj->size[0] = min_tmplt[2];
173 obj->size[1] = min_tmplt[1];
174 obj->size[2] = min_tmplt[0];
175 } else if (volume_object > max_template_volume ||
176 obj->size[2] >
177 (1 + ref_param_.height_diff_ratio()) * max_tmplt[0]) {
178 obj->size[0] = max_tmplt[2];
179 obj->size[1] = max_tmplt[1];
180 obj->size[2] = max_tmplt[0];
181 }
182 }
183
184 if (Contain(object_template_manager_->TypeRefinedByRef(), obj->sub_type)) {
185 std::string sensor = frame->data_provider->sensor_name();
186 std::vector<float> height;
187
188 double z_obj = frame->camera_k_matrix(1, 1) * obj->size[2] /
189 (supplement.box.ymax - supplement.box.ymin);
190
191 // h from ground detection
192 SyncGroundEstimator(sensor, frame->camera_k_matrix,
193 static_cast<int>(img_width_),
194 static_cast<int>(img_height_));
195 auto &ground_estimator = ground_estimator_mapper_[sensor];
196 float l[3] = {0};
197 if (ground_estimator.GetGroundModel(l)) {
198 double z_ground_reversed =
199 (-l[0] * supplement.box.ymax - l[2]) * algorithm::IRec(l[1]);
200 double z_ground = algorithm::IRec(z_ground_reversed);
201 z_ground = std::max(z_ground, z_obj);
202 float k2 = static_cast<float>(z_ground / frame->camera_k_matrix(1, 1));
203 float h = k2 * (supplement.box.ymax - supplement.box.ymin);
204 h = std::max(std::min(h, kMaxTemplateHWL.at(obj->sub_type).at(0)),
205 kMinTemplateHWL.at(obj->sub_type).at(0));
206 height.push_back(h);
207 }
208
209 // h from calibration service
210 if (frame->calibration_service != nullptr) {
211 double z_calib = 0.0;
212 bool success = frame->calibration_service->QueryDepthOnGroundPlane(
213 static_cast<int>(supplement.box.Center().x),
214 static_cast<int>(supplement.box.ymax), &z_calib);
215 if (success) {
216 z_calib = std::max(z_calib, z_obj);
217 float k2 = static_cast<float>(z_calib / frame->camera_k_matrix(1, 1));
218 float h = k2 * (supplement.box.ymax - supplement.box.ymin);
219 h = std::max(std::min(h, kMaxTemplateHWL.at(obj->sub_type).at(0)),
220 kMinTemplateHWL.at(obj->sub_type).at(0));
221 height.push_back(h);
222 } else {
223 height.push_back(kMaxTemplateHWL.at(obj->sub_type).at(0));
224 }
225 Eigen::Vector4d plane_param;
226 // wxt todo: solve this problem
227 // frame->calibration_service->QueryGroundPlaneInCameraTrackingFrame(&plane_param);
228 }
229
230 // h from history reference vote
231 if (obj->sub_type != base::ObjectSubType::TRAFFICCONE || height.empty()) {
232 float cy = frame->camera_k_matrix(1, 2);
233 cy = std::min(supplement.box.ymax - 1, cy);
234 for (auto &&reference : reference_[sensor]) {
235 // pick out the refs close enough in y directions
236 float dy = std::abs(reference.ymax - supplement.box.ymax);
237 float scale_y = (supplement.box.ymax - cy) / (img_height_ - cy);
238 float thres_dy =
239 std::max(static_cast<float>(ref_param_.down_sampling()) * scale_y,
240 static_cast<float>(ref_param_.margin()) * 2.0f);
241 if (dy < thres_dy) {
242 float k2 = reference.k;
243 k2 *= (reference.ymax - cy) / (supplement.box.ymax - cy);
244 float h = k2 * (supplement.box.ymax - supplement.box.ymin);
245 // bounded by templates
246 h = std::max(std::min(h, kMaxTemplateHWL.at(obj->sub_type).at(0)),
247 kMinTemplateHWL.at(obj->sub_type).at(0));
248 height.push_back(h);
249 }
250 }
251 height.push_back(obj->size[2]);
252 }
253
254 if (height.empty()) {
255 AERROR << "height vector is empty";
256 continue;
257 }
258 std::sort(height.begin(), height.end());
259 int nr_hs = static_cast<int>(height.size());
260 float h_updated = height[nr_hs / 2];
261 AINFO << "Estimate " << h_updated << " with " << height.size();
262 obj->size[2] =
263 std::min(h_updated, kMaxTemplateHWL.at(obj->sub_type).at(0));
264 obj->size[2] =
265 std::max(obj->size[2], kMinTemplateHWL.at(obj->sub_type).at(0));
266 std::vector<float> height_error(kTemplateHWL.size(), 0);
267
268 float obj_h = obj->size[2];
269 for (size_t i = 0; i < height_error.size(); ++i) {
270 height_error[i] =
271 std::abs(kTemplateHWL[i].at(obj->sub_type).at(0) - obj_h);
272 }
273 auto min_error =
274 std::min_element(height_error.begin(), height_error.end());
275 auto min_index = std::distance(height_error.begin(), min_error);
276 auto &tmplt = kTemplateHWL.at(min_index).at(obj->sub_type);
277 float scale_factor = obj->size[2] / tmplt[0];
278 obj->size[1] = tmplt[1] * scale_factor;
279 obj->size[0] = tmplt[2] * scale_factor;
280 }
281 ADEBUG << "correct size:" << obj->size.transpose();
282 }
283}
apollo::perception::camera::ObjectTemplateManager::TypeRefinedByTemplate
const std::vector< base::ObjectSubType > & TypeRefinedByTemplate()
Definition object_template_manager.h:67
apollo::perception::camera::ObjectTemplateManager::TypeRefinedByRef
const std::vector< base::ObjectSubType > & TypeRefinedByRef()
Definition object_template_manager.h:71
ADEBUG
#define ADEBUG
Definition log.h:41
AERROR
#define AERROR
Definition log.h:44
AINFO
#define AINFO
Definition log.h:42
apollo::drivers::robosense::height
uint32_t height
Height of point cloud
Definition point_cloud_msg.h:53
apollo::perception::camera::Contain
bool Contain(const std::vector< T > &array, const T &element)
Definition util.h:64
apollo::perception::camera::TemplateMap
std::map< base::ObjectSubType, std::vector< float > > TemplateMap
Definition object_template_manager.h:43

◆ Init()

void apollo::perception::camera::ObstacleReference::Init ( const ReferenceParam &  ref_param,
float  width,
float  height 
)

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

31 {
32 ref_param_ = ref_param;
33 img_width_ = width;
34 img_height_ = height;
35 ref_width_ = static_cast<int>(
36 width / static_cast<float>(ref_param.down_sampling()) + 1);
37 ref_height_ = static_cast<int>(
38 height / static_cast<float>(ref_param.down_sampling()) + 1);
39 static const float k =
40 static_cast<float>(ref_height_) / static_cast<float>(ref_width_);
41 static const float b = static_cast<float>(ref_height_);
42 init_ref_map_.resize(ref_height_);
43 for (int y = 0; y < ref_height_; ++y) {
44 init_ref_map_[y].resize(ref_width_, -1);
45 if (y < ref_height_ / 2 || y > ref_height_ - ref_param_.margin()) {
46 continue;
47 }
48 for (int x = ref_param.margin(); x < ref_width_ - ref_param.margin(); ++x) {
49 if (y > -k * static_cast<float>(x) + b && y > k * static_cast<float>(x)) {
50 init_ref_map_[y][x] = 0;
51 }
52 }
53 }
54 object_template_manager_ = ObjectTemplateManager::Instance();
55}
apollo::drivers::robosense::width
uint32_t width
Width of point cloud
Definition point_cloud_msg.h:54

◆ UpdateReference()

void apollo::perception::camera::ObstacleReference::UpdateReference ( const CameraTrackingFrame *  frame,
const apollo::common::EigenVector< Target > &  targets 
)

在文件 obstacle_reference.cc 第 57 行定义.

58 {
59 std::string sensor = frame->data_provider->sensor_name();
60 SyncGroundEstimator(sensor, frame->camera_k_matrix,
61 static_cast<int>(img_width_),
62 static_cast<int>(img_height_));
63 auto &ground_estimator = ground_estimator_mapper_[sensor];
64
65 auto &refs = reference_[sensor];
66 auto &ref_map = ref_map_[sensor];
67 if (ref_map.empty()) {
68 ref_map = init_ref_map_;
69 }
70 for (auto &&reference : refs) {
71 reference.area *= ref_param_.area_decay();
72 }
73 for (auto &&target : targets) {
74 if (!target.isTracked()) {
75 continue;
76 }
77 if (target.isLost()) {
78 continue;
79 }
80 auto obj = target[-1]->object;
81 if (!Contain(object_template_manager_->TypeCanBeRef(), obj->sub_type)) {
82 continue;
83 }
84 auto &box = obj->camera_supplement.box;
85
86 if (box.ymax - box.ymin < ref_param_.min_allow_height()) {
87 continue;
88 }
89 if (box.ymax < frame->camera_k_matrix(1, 2) + 1) {
90 continue;
91 }
92 if (box.ymax >= img_height_ + 1) {
93 AERROR << "box.ymax (" << box.ymax << ") is larger than img_height_ + 1 ("
94 << img_height_ + 1 << ")";
95 return;
96 }
97 if (box.xmax >= img_width_ + 1) {
98 AERROR << "box.xmax (" << box.xmax << ") is larger than img_width_ + 1 ("
99 << img_width_ + 1 << ")";
100 return;
101 }
102 float x = box.Center().x;
103 float y = box.ymax;
104
105 AINFO << "Target: " << target.id
106 << " can be Ref. Type: " << base::kSubType2NameMap.at(obj->sub_type);
107 int y_discrete =
108 static_cast<int>(y / static_cast<float>(ref_param_.down_sampling()));
109 int x_discrete =
110 static_cast<int>(x / static_cast<float>(ref_param_.down_sampling()));
111 if (ref_map[y_discrete][x_discrete] == 0) {
112 Reference r;
113 r.area = box.Area();
114 r.ymax = y;
115 r.k = obj->size[2] / (y - box.ymin);
116 refs.push_back(r);
117 ref_map[y_discrete][x_discrete] = static_cast<int>(refs.size());
118 } else if (ref_map[y_discrete][x_discrete] > 0) {
119 int index = ref_map[y_discrete][x_discrete] - 1;
120 if (box.Area() > refs[index].area) {
121 refs[index].area = box.Area();
122 refs[index].ymax = y;
123 refs[index].k = obj->size[2] / (y - box.ymin);
124 }
125 }
126 }
127
128 // detect the ground from reference
129 std::vector<float> vd_samples(static_cast<int>(refs.size() * 2), 0);
130 int count_vd = 0;
131 for (auto &&reference : refs) {
132 float z_ref = reference.k * frame->camera_k_matrix(1, 1);
133 vd_samples[count_vd++] = reference.ymax;
134 vd_samples[count_vd++] = algorithm::IRec(z_ref);
135 }
136
137 int count_samples = static_cast<int>(vd_samples.size() / 2);
138 if (count_samples > ground_estimator.get_min_nr_samples() &&
139 frame->calibration_service != nullptr) {
140 ground_estimator.DetetGround(
141 frame->calibration_service->QueryPitchAngle(),
142 frame->calibration_service->QueryCameraToGroundHeight(),
143 vd_samples.data(), count_samples);
144 }
145}
apollo::perception::camera::ObjectTemplateManager::TypeCanBeRef
const std::vector< base::ObjectSubType > & TypeCanBeRef()
Definition object_template_manager.h:75
apollo::perception::base::kSubType2NameMap
const std::map< ObjectSubType, std::string > kSubType2NameMap
Definition object_types.h:249

类成员变量说明

◆ object_template_manager_

ObjectTemplateManager* apollo::perception::camera::ObstacleReference::object_template_manager_ = nullptr
protected

在文件 obstacle_reference.h 第 77 行定义.

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