Apollo 11.0
自动驾驶开放平台
Public 成员函数 | Protected 成员函数 | 所有成员列表
apollo::perception::lidar::MlfMotionMeasurement类 参考

#include <mlf_motion_measurement.h>

apollo::perception::lidar::MlfMotionMeasurement 的协作图:

Public 成员函数

 MlfMotionMeasurement ()=default
 
 ~MlfMotionMeasurement ()=default
 
void ComputeMotionMeasurment (const MlfTrackDataConstPtr &track_data, TrackedObjectPtr new_object)
 Wrapper of motion measurement functions
 

Protected 成员函数

void MeasurementSelection (const MlfTrackDataConstPtr &track_data, const TrackedObjectConstPtr &latest_object, TrackedObjectPtr new_object, bool condition=true)
 Select measurement based on track history
 
void MeasurementQualityEstimation (const TrackedObjectConstPtr &latest_object, TrackedObjectPtr new_object)
 Estimate measurement quality
 
void MeasurementRefine (const MlfTrackDataConstPtr &track_data, const TrackedObjectConstPtr &latest_object, TrackedObjectPtr new_object, const double &time_diff)
 
Eigen::Vector2d ComputeExpectedVelocity (std::vector< TrackedObjectConstPtr > history_objects, double cur_time)
 

详细描述

在文件 mlf_motion_measurement.h 第 28 行定义.

构造及析构函数说明

◆ MlfMotionMeasurement()

apollo::perception::lidar::MlfMotionMeasurement::MlfMotionMeasurement ( )
default

◆ ~MlfMotionMeasurement()

apollo::perception::lidar::MlfMotionMeasurement::~MlfMotionMeasurement ( )
default

成员函数说明

◆ ComputeExpectedVelocity()

Eigen::Vector2d apollo::perception::lidar::MlfMotionMeasurement::ComputeExpectedVelocity ( std::vector< TrackedObjectConstPtr >  history_objects,
double  cur_time 
)
protected

在文件 mlf_motion_measurement.cc 第 379 行定义.

381 {
382 if (history_objects.size() == 1) {
383 return Eigen::Vector2d(0, 0);
384 }
385 size_t valid = 0;
386 double velocity_sum_x = 0.0, velocity_sum_y = 0.0;
387 for (size_t i = 0; i < history_objects.size(); i++) {
388 auto after_obj = history_objects[i];
389 for (size_t j = i + 1; j < history_objects.size(); j++) {
390 // index protect
391 if (i == j || j >= history_objects.size()) {
392 continue;
393 }
394 auto pre_obj = history_objects[j];
395 if (after_obj->output_velocity.head<2>().norm() < STATIC_VELOCITY
396 || pre_obj->output_velocity.head<2>().norm() < STATIC_VELOCITY) {
397 continue;
398 }
399 valid++;
400 double time_ratio = after_obj->object_ptr->latest_tracked_time -
401 pre_obj->object_ptr->latest_tracked_time;
402 double ratio_x = (after_obj->output_velocity(0) -
403 pre_obj->output_velocity(0)) / (time_ratio);
404 double ratio_y = (after_obj->output_velocity(1) -
405 pre_obj->output_velocity(1)) / (time_ratio);
406
407 double expected_time_diff = cur_time -
408 after_obj->object_ptr->latest_tracked_time;
409 velocity_sum_x += ratio_x * expected_time_diff +
410 after_obj->output_velocity(0);
411 velocity_sum_y += ratio_y * expected_time_diff +
412 after_obj->output_velocity(1);
413 }
414 }
415 if (valid == 0) {
416 return Eigen::Vector2d(0, 0);
417 }
418 velocity_sum_x /= valid;
419 velocity_sum_y /= valid;
420 return Eigen::Vector2d(velocity_sum_x, velocity_sum_y);
421}

◆ ComputeMotionMeasurment()

void apollo::perception::lidar::MlfMotionMeasurement::ComputeMotionMeasurment ( const MlfTrackDataConstPtr &  track_data,
TrackedObjectPtr  new_object 
)

Wrapper of motion measurement functions

参数
track_datatrack data
new_objectobject for current updating

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

34 {
35 // prefer to choose objects from the same sensor
36 std::string sensor_name = new_object->sensor_info.name;
37 TrackedObjectConstPtr latest_object =
38 track_data->GetLatestSensorObject(sensor_name).second;
39 if (latest_object == nullptr) {
40 latest_object = track_data->GetLatestObject().second;
41 }
42 if (latest_object.get() == nullptr) {
43 AERROR << "latest_object is not available";
44 return;
45 }
46 // should we estimate the measurement if the time diff is too small?
47 double latest_time = latest_object->object_ptr->latest_tracked_time;
48 double current_time = new_object->object_ptr->latest_tracked_time;
49 double time_diff = current_time - latest_time;
50 if (fabs(time_diff) < EPSILON_TIME) {
51 time_diff = DEFAULT_FPS;
52 }
53 MeasureAnchorPointVelocity(new_object, latest_object, time_diff);
54 MeasureBboxCenterVelocity(new_object, latest_object, time_diff);
55 MeasureBboxCornerVelocity(new_object, latest_object, time_diff);
56 MeasureBboxCenterHistoryVelocity(track_data, new_object);
57 bool spec_condition =
58 new_object->object_ptr->sub_type == base::ObjectSubType::BIGMOT;
59 if (spec_condition) {
60 MeasureBboxCornerHistoryVelocity(track_data, new_object);
61 MeasureObjectDetectionHistoryVelocity(track_data, new_object);
62 MeasureObjectDetectionVelocity(new_object, latest_object, time_diff);
63 }
64 MeasurementSelection(track_data, latest_object, new_object, spec_condition);
65 MeasurementQualityEstimation(latest_object, new_object);
66 MeasurementRefine(track_data, latest_object, new_object, time_diff);
67}
apollo::perception::lidar::MlfMotionMeasurement::MeasurementRefine
void MeasurementRefine(const MlfTrackDataConstPtr &track_data, const TrackedObjectConstPtr &latest_object, TrackedObjectPtr new_object, const double &time_diff)
Definition mlf_motion_measurement.cc:233
apollo::perception::lidar::MlfMotionMeasurement::MeasurementQualityEstimation
void MeasurementQualityEstimation(const TrackedObjectConstPtr &latest_object, TrackedObjectPtr new_object)
Estimate measurement quality
Definition mlf_motion_measurement.cc:215
apollo::perception::lidar::MlfMotionMeasurement::MeasurementSelection
void MeasurementSelection(const MlfTrackDataConstPtr &track_data, const TrackedObjectConstPtr &latest_object, TrackedObjectPtr new_object, bool condition=true)
Select measurement based on track history
Definition mlf_motion_measurement.cc:69
AERROR
#define AERROR
Definition log.h:44
apollo::perception::lidar::TrackedObjectConstPtr
std::shared_ptr< const TrackedObject > TrackedObjectConstPtr
Definition tracked_object.h:231
apollo::perception::lidar::MeasureBboxCornerVelocity
void MeasureBboxCornerVelocity(TrackedObjectPtr new_object, const TrackedObjectConstPtr &old_object, const double &time_diff)
Measure bbox corner velocity
Definition measurement_collection.cc:104
apollo::perception::lidar::MeasureBboxCornerHistoryVelocity
void MeasureBboxCornerHistoryVelocity(const MlfTrackDataConstPtr &track_data, TrackedObjectPtr new_object)
Definition measurement_collection.cc:199
apollo::perception::lidar::MeasureBboxCenterHistoryVelocity
void MeasureBboxCenterHistoryVelocity(const MlfTrackDataConstPtr &track_data, TrackedObjectPtr new_object)
Definition measurement_collection.cc:69
apollo::perception::lidar::MeasureObjectDetectionHistoryVelocity
void MeasureObjectDetectionHistoryVelocity(const MlfTrackDataConstPtr &track_data, TrackedObjectPtr new_object)
Definition measurement_collection.cc:322
apollo::perception::lidar::MeasureObjectDetectionVelocity
void MeasureObjectDetectionVelocity(TrackedObjectPtr new_object, const TrackedObjectConstPtr &old_object, const double &time_diff)
Measure object-detection center and corner velocity
Definition measurement_collection.cc:313
apollo::perception::lidar::MeasureAnchorPointVelocity
void MeasureAnchorPointVelocity(TrackedObjectPtr new_object, const TrackedObjectConstPtr &old_object, const double &time_diff)
Measure anchor point velocity
Definition measurement_collection.cc:28
apollo::perception::lidar::MeasureBboxCenterVelocity
void MeasureBboxCenterVelocity(TrackedObjectPtr new_object, const TrackedObjectConstPtr &old_object, const double &time_diff)
Measure bbox center velocity
Definition measurement_collection.cc:39

◆ MeasurementQualityEstimation()

void apollo::perception::lidar::MlfMotionMeasurement::MeasurementQualityEstimation ( const TrackedObjectConstPtr &  latest_object,
TrackedObjectPtr  new_object 
)
protected

Estimate measurement quality

参数
latest_objectlatest object in track
new_objectnew object for storing quality

在文件 mlf_motion_measurement.cc 第 215 行定义.

216 {
217 // 1. point size diff (only for same sensor)
218 int pre_num = static_cast<int>(
219 latest_object->object_ptr->lidar_supplement.cloud_world.size());
220 int cur_num = static_cast<int>(
221 new_object->object_ptr->lidar_supplement.cloud_world.size());
222 double quality_based_on_point_diff_ratio =
223 (latest_object->sensor_info.name == new_object->sensor_info.name)
224 ? (1.0 - fabs(pre_num - cur_num) / std::max(pre_num, cur_num))
225 : 1.0;
226 // 2. association quality
227 double quality_based_on_association_score =
228 pow(1.0 - new_object->association_score, 2.0);
229 new_object->update_quality = std::min(quality_based_on_association_score,
230 quality_based_on_point_diff_ratio);
231}

◆ MeasurementRefine()

void apollo::perception::lidar::MlfMotionMeasurement::MeasurementRefine ( const MlfTrackDataConstPtr &  track_data,
const TrackedObjectConstPtr &  latest_object,
TrackedObjectPtr  new_object,
const double &  time_diff 
)
protected

在文件 mlf_motion_measurement.cc 第 233 行定义.

237 {
238 const Eigen::Vector3d cur_vel = new_object->selected_measured_velocity;
239 const int get_history_size = 4;
240 // get hisroty objects
241 std::vector<TrackedObjectConstPtr> history_objects;
242 track_data->GetLatestKObjects(get_history_size, &history_objects);
243 if (!history_objects.size()) {
244 AWARN << "No history objects find. Unreasonable";
245 return;
246 }
247
248 float velocity_threshold = 2.0;
249 velocity_threshold =
250 latest_object->type == base::ObjectType::VEHICLE ? 3.0 : 2.0;
251
252 float small_velocity_threshold = 2.0;
253 small_velocity_threshold =
254 latest_object->type == base::ObjectType::PEDESTRIAN ? 0.1 : 0.5;
255 if (cur_vel.head<2>().norm() > velocity_threshold) {
256 ++new_object->measured_big_velocity_age;
257 }
258 if (cur_vel.head<2>().norm() < small_velocity_threshold) {
259 new_object->measured_big_velocity_age = 0;
260 }
261
262 // Condition 1: size < 3 and latest is static and new velocity is big
263 if (history_objects.size() < get_history_size) {
264 if (latest_object->output_velocity.head<2>().norm() < STATIC_VELOCITY
265 && cur_vel.head<2>().norm() > velocity_threshold) {
266 // new_object->selected_measured_velocity =
267 // latest_object->selected_measured_velocity;
268 new_object->selected_measured_velocity =
269 latest_object->output_velocity;
270 AINFO << "[Velocity-Measurement] track_id: "
271 << track_data->track_id_
272 << " history small and current BIG. Keep Lastest Output";
273 return;
274 } else {
275 return;
276 }
277 }
278
279 // Condition 2: latest-three static (including small velocity)
280 // and new_velocity is big
281 bool keep_small_velocity = true;
282 for (auto& obj : history_objects) {
283 if (obj->output_velocity.head<2>().norm() > 0.1) {
284 keep_small_velocity = false;
285 }
286 }
287 if (new_object->measured_big_velocity_age >= get_history_size) {
288 AINFO << "[Velocity-Measurement] Continuous big velocity. track_id: "
289 << track_data->track_id_;
290 return;
291 }
292 if (keep_small_velocity && cur_vel.head<2>().norm() > velocity_threshold) {
293 // new_object->selected_measured_velocity =
294 // latest_object->selected_measured_velocity;
295 new_object->selected_measured_velocity =
296 latest_object->output_velocity;
297 AINFO << "[Velocity-Measurement] track_id: " << track_data->track_id_
298 << " history static and current BIG. Keep Lastest Output";
299 return;
300 }
301
302 // Condition 3: average and variance change BIG
303 size_t ori_size = history_objects.size();
304 size_t new_size = ori_size + 1;
305 Eigen::Vector3d old_ave = Eigen::Vector3d::Zero();
306 Eigen::Vector3d old_var = Eigen::Vector3d::Zero();
307 Eigen::Vector3d new_ave = Eigen::Vector3d::Zero();
308 Eigen::Vector3d new_var = Eigen::Vector3d::Zero();
309 // Condition 3: Small-Velocity and small-velocity-variance and SUDDENLY BIG
310 for (auto& obj : history_objects) {
311 old_ave += obj->output_velocity;
312 }
313 old_ave /= ori_size;
314 for (auto& obj : history_objects) {
315 old_var(0) += ((obj->output_velocity(0) - old_ave(0)) *
316 (obj->output_velocity(0) - old_ave(0)));
317 old_var(1) += ((obj->output_velocity(1) - old_ave(1)) *
318 (obj->output_velocity(1) - old_ave(1)));
319 }
320 old_var /= ori_size;
321
322 // old and cur to get new
323 new_ave = old_ave + (cur_vel - old_ave) * 1.0 / ori_size;
324 new_var(0) = (cur_vel(0) - old_ave(0)) * (cur_vel(0) - old_ave(0)) * 1.0 *
325 ori_size / (new_size * new_size)
326 + old_var(0) * 1.0 * ori_size / new_size;
327 new_var(1) = (cur_vel(1) - old_ave(1)) * (cur_vel(1) - old_ave(1)) * 1.0 *
328 ori_size / (new_size * new_size)
329 + old_var(1) * 1.0 * ori_size / new_size;
330
331 float ave_change = fabs(new_ave.head<2>().norm() -
332 old_ave.head<2>().norm());
333 float var_change = fabs(new_var.head<2>().norm() -
334 old_var.head<2>().norm());
335 if (ave_change > 2.0 && var_change > 2.0) {
336 // new_object->selected_measured_velocity =
337 // latest_object->selected_measured_velocity;
338 new_object->selected_measured_velocity =
339 latest_object->output_velocity;
340 AINFO << "[Velocity-Measurement] track_id: " << track_data->track_id_
341 << " old_ave: " << old_ave(0) << ", " << old_ave(1)
342 << " old_var: " << old_var(0) << ", " << old_var(1)
343 << " new_ave: " << new_ave(0) << ", " << new_ave(1)
344 << " new_var: " << new_var(0) << ", " << new_var(1)
345 << " Change Bigger. Keep Lastest Output";
346 return;
347 }
348
349 // Condition 4: disverge bigger (Vt = V0 + a * delta_T)
350 double cur_time = new_object->object_ptr->latest_tracked_time;
351 Eigen::Vector2d expected_velocity =
352 ComputeExpectedVelocity(history_objects, cur_time);
353 bool ratio_flag = cur_vel.head<2>().norm() / expected_velocity.norm() > 20;
354 if (expected_velocity.norm() > STATIC_VELOCITY && ratio_flag) {
355 // new_object->selected_measured_velocity =
356 // latest_object->selected_measured_velocity;
357 new_object->selected_measured_velocity =
358 latest_object->output_velocity;
359 AINFO << "[Velocity-Measurement] track_id: " << track_data->track_id_
360 << " Expected velocity CHANGE big. Keep Lastest Output";
361 return;
362 }
363
364 // Condition 5: direction diff BIG [Not Use]
365 // Eigen::Vector3f cur_dir = new_object->object_ptr->direction;
366 // cur_dir.normalize();
367 // Eigen::Vector3f pre_dir = latest_object->object_ptr->direction;
368 // pre_dir.normalize();
369 // float theta_change = algorithm::CalculateTheta2DXY(pre_dir, cur_dir);
370 // // if (fabs(theta_change) > M_PI / 4) {
371 // // new_object->selected_measured_velocity =
372 // // latest_object->selected_measured_velocity;
373 // // sstr << " direction CHANGE BIG. KEEP LATEST MEASURE";
374 // // ADEBUG << sstr.str();
375 // // return;
376 // // }
377}
apollo::perception::lidar::MlfMotionMeasurement::ComputeExpectedVelocity
Eigen::Vector2d ComputeExpectedVelocity(std::vector< TrackedObjectConstPtr > history_objects, double cur_time)
Definition mlf_motion_measurement.cc:379
AINFO
#define AINFO
Definition log.h:42
AWARN
#define AWARN
Definition log.h:43

◆ MeasurementSelection()

void apollo::perception::lidar::MlfMotionMeasurement::MeasurementSelection ( const MlfTrackDataConstPtr &  track_data,
const TrackedObjectConstPtr &  latest_object,
TrackedObjectPtr  new_object,
bool  condition = true 
)
protected

Select measurement based on track history

参数
track_datatrack data
latest_objectlatest object in track
new_objectnew object for storing selection
conditionsome special condition for selection

在文件 mlf_motion_measurement.cc 第 69 行定义.

72 {
73 // Select measured velocity among candidates according motion consistency
74 int64_t corner_index = 0;
75 float corner_velocity_gain = 0.0f;
76 std::vector<float> corner_velocity_gain_norms(4);
77 for (int i = 0; i < 4; ++i) {
78 corner_velocity_gain_norms[i] =
79 static_cast<float>((new_object->measured_corners_velocity[i] -
80 latest_object->belief_velocity)
81 .norm());
82 }
83 std::vector<float>::iterator corener_min_gain =
84 std::min_element(std::begin(corner_velocity_gain_norms),
85 std::end(corner_velocity_gain_norms));
86 corner_velocity_gain = *corener_min_gain;
87 corner_index = corener_min_gain - corner_velocity_gain_norms.begin();
88
89 std::vector<float> velocity_gain_norms(4);
90 velocity_gain_norms[0] = corner_velocity_gain;
91 velocity_gain_norms[1] =
92 static_cast<float>((new_object->measured_barycenter_velocity -
93 latest_object->belief_velocity)
94 .norm());
95 velocity_gain_norms[2] = static_cast<float>(
96 (new_object->measured_center_velocity - latest_object->belief_velocity)
97 .norm());
98 velocity_gain_norms[3] = static_cast<float>(
99 (new_object->measured_history_center_velocity -
100 latest_object->belief_velocity).norm());
101 std::vector<float>::iterator min_gain = std::min_element(
102 std::begin(velocity_gain_norms), std::end(velocity_gain_norms));
103 int64_t min_gain_pos = min_gain - velocity_gain_norms.begin();
104 if (min_gain_pos == 0) {
105 new_object->selected_measured_velocity =
106 new_object->measured_corners_velocity[corner_index];
107 }
108 if (min_gain_pos == 1) {
109 new_object->selected_measured_velocity =
110 new_object->measured_barycenter_velocity;
111 }
112 if (min_gain_pos == 2) {
113 new_object->selected_measured_velocity =
114 new_object->measured_center_velocity;
115 }
116 if (min_gain_pos == 3) {
117 new_object->selected_measured_velocity =
118 new_object->measured_history_center_velocity;
119 }
120 if (condition) {
121 // object-detection-corner-history-observation
122 std::vector<float> condition_velocity_norm(10);
123 for (int i = 0; i < 4; ++i) {
124 condition_velocity_norm[i] = static_cast<float>(
125 (new_object->measured_detection_history_corners_velocity[i] -
126 latest_object->belief_velocity).norm());
127 }
128 // object-detection-center-observation
129 condition_velocity_norm[4] = static_cast<float>(
130 (new_object->measured_detection_history_center_velocity -
131 latest_object->belief_velocity).norm());
132 condition_velocity_norm[5] = static_cast<float>(
133 (new_object->measured_detection_center_velocity -
134 latest_object->belief_velocity).norm());
135 // corners-history-observation
136 for (int i = 0; i < 4; ++i) {
137 condition_velocity_norm[i + 6] = static_cast<float>(
138 (new_object->measured_history_corners_velocity[i] -
139 latest_object->belief_velocity).norm());
140 }
141 std::vector<float>::iterator condition_min_diff =
142 std::min_element(std::begin(condition_velocity_norm),
143 std::end(condition_velocity_norm));
144 int min_pos = condition_min_diff - condition_velocity_norm.begin();
145 if (*condition_min_diff < *min_gain) {
146 if (min_pos <= 3 && min_pos >= 0) {
147 new_object->selected_measured_velocity =
148 new_object->measured_detection_history_corners_velocity[min_pos];
149 }
150 if (min_pos == 4) {
151 new_object->selected_measured_velocity =
152 new_object->measured_detection_history_center_velocity;
153 }
154 if (min_pos == 5) {
155 new_object->selected_measured_velocity =
156 new_object->measured_detection_center_velocity;
157 }
158 if (min_pos <= 9 && min_pos >= 6) {
159 new_object->selected_measured_velocity =
160 new_object->measured_history_corners_velocity[min_pos - 6];
161 }
162 }
163 }
164 ADEBUG << "[Measurement] track_id: " << track_data->track_id_
165 << " belief_velocity: " << latest_object->belief_velocity(0)
166 << ", " << latest_object->belief_velocity(1)
167 << " corner_velocity[0]: " << new_object->measured_corners_velocity[0](0)
168 << ", " << new_object->measured_corners_velocity[0](1)
169 << " corner_velocity[1]: " << new_object->measured_corners_velocity[1](0)
170 << ", " << new_object->measured_corners_velocity[1](1)
171 << " corner_velocity[2]: " << new_object->measured_corners_velocity[2](0)
172 << ", " << new_object->measured_corners_velocity[2](1)
173 << " corner_velocity[3]: " << new_object->measured_corners_velocity[3](0)
174 << ", " << new_object->measured_corners_velocity[3](1)
175 << " barycenter_velocity: " << new_object->measured_barycenter_velocity(0)
176 << ", " << new_object->measured_barycenter_velocity(1)
177 << " center_velocity: " << new_object->measured_center_velocity(0)
178 << ", " << new_object->measured_center_velocity(1)
179 << " history_velocity: "<< new_object->measured_history_center_velocity(0)
180 << ", " << new_object->measured_history_center_velocity(1)
181 << " detection_history_corner_velocity[0]: "
182 << new_object->measured_detection_history_corners_velocity[0](0) << ", "
183 << new_object->measured_detection_history_corners_velocity[0](1)
184 << " detection_history_corner_velocity[1]: "
185 << new_object->measured_detection_history_corners_velocity[1](0) << ", "
186 << new_object->measured_detection_history_corners_velocity[1](1)
187 << " detection_history_corner_velocity[2]: "
188 << new_object->measured_detection_history_corners_velocity[2](0) << ", "
189 << new_object->measured_detection_history_corners_velocity[2](1)
190 << " detection_history_corner_velocity[3]: "
191 << new_object->measured_detection_history_corners_velocity[3](0) << ", "
192 << new_object->measured_detection_history_corners_velocity[3](1)
193 << " detection_center_velocity: "
194 << new_object->measured_detection_center_velocity(0) << ", "
195 << new_object->measured_detection_center_velocity(1)
196 << " detection_history_center_velocity: "
197 << new_object->measured_detection_history_center_velocity(0) << ", "
198 << new_object->measured_detection_history_center_velocity(1)
199 << " history_corner_velocity[0]: "
200 << new_object->measured_history_corners_velocity[0](0) << ", "
201 << new_object->measured_history_corners_velocity[0](1)
202 << " history_corner_velocity[1]: "
203 << new_object->measured_history_corners_velocity[1](0) << ", "
204 << new_object->measured_history_corners_velocity[1](1)
205 << " history_corner_velocity[2]: "
206 << new_object->measured_history_corners_velocity[2](0) << ", "
207 << new_object->measured_history_corners_velocity[2](1)
208 << " history_corner_velocity[3]: "
209 << new_object->measured_history_corners_velocity[3](0) << ", "
210 << new_object->measured_history_corners_velocity[3](1)
211 << " selected: " << new_object->selected_measured_velocity(0) << ", "
212 << new_object->selected_measured_velocity(1);
213}
ADEBUG
#define ADEBUG
Definition log.h:41
该类的文档由以下文件生成: