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

#include <mlf_motion_refiner.h>

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

Public 成员函数

 MlfMotionRefiner ()=default
 
 ~MlfMotionRefiner ()=default
 
bool Init (const MlfMotionRefinerInitOptions &options=MlfMotionRefinerInitOptions())
 Init motion refiner
 
bool Refine (const MlfTrackDataConstPtr &track_data, TrackedObjectPtr new_object)
 Refine velocity of new object
 
std::string Name () const
 Get class name
 

Protected 成员函数

bool CheckStaticHypothesisByState (const TrackedObjectConstPtr &latest_object, const TrackedObjectConstPtr &new_object) const
 Check whether new observed object is static or not
 
bool CheckStaticHypothesisByVelocityAngleChange (const TrackedObjectConstPtr &latest_object, const TrackedObjectConstPtr &new_object, double reasonable_angle_change_maximum) const
 Check whether new observed object is static or not via considering the angle velocity change
 
bool CalculateVelocityAngleChange (const TrackedObjectConstPtr &latest_object, const TrackedObjectConstPtr &new_object, double *vel_change_angle, double *velocity_heading_angle) const
 
bool NeedAssignMovableObjectSpeed (const TrackedObjectConstPtr &latest_object, const TrackedObjectConstPtr &new_object) const
 

Protected 属性

double claping_acceleration_threshold_ = 10
 
double claping_speed_threshold_ = 1.0
 
double cyc_refine_speed_ = 1.2
 
double car_refine_speed_ = 1.0
 
const double EPSION_TIME = 1e-3
 
bool use_movable_state_check_ = false
 

详细描述

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

构造及析构函数说明

◆ MlfMotionRefiner()

apollo::perception::lidar::MlfMotionRefiner::MlfMotionRefiner ( )
default

◆ ~MlfMotionRefiner()

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

成员函数说明

◆ CalculateVelocityAngleChange()

bool apollo::perception::lidar::MlfMotionRefiner::CalculateVelocityAngleChange ( const TrackedObjectConstPtr &  latest_object,
const TrackedObjectConstPtr &  new_object,
double *  vel_change_angle,
double *  velocity_heading_angle 
) const
protected

在文件 mlf_motion_refiner.cc 第 180 行定义.

184 {
185 Eigen::Vector3d pre_velocity = latest_object->output_velocity;
186 Eigen::Vector3d cur_velocity = new_object->output_velocity;
187 Eigen::Vector3d pre_direction = latest_object->output_direction;
188 constexpr double kEpsilon = std::numeric_limits<double>::epsilon();
189
190 if (pre_velocity.norm() < kEpsilon || cur_velocity.norm() < kEpsilon) {
191 return false;
192 }
193 *vel_change_angle = algorithm::CalculateTheta2DXY<double>(
194 pre_velocity, cur_velocity);
195
196 double velocity_heading_angle_1 = algorithm::CalculateTheta2DXY<double>(
197 pre_direction, cur_velocity);
198 pre_direction *= -1;
199 double velocity_heading_angle_2 = algorithm::CalculateTheta2DXY<double>(
200 pre_direction, cur_velocity);
201 *velocity_heading_angle = std::min(fabs(velocity_heading_angle_1),
202 fabs(velocity_heading_angle_2));
203 return true;
204}

◆ CheckStaticHypothesisByState()

bool apollo::perception::lidar::MlfMotionRefiner::CheckStaticHypothesisByState ( const TrackedObjectConstPtr &  latest_object,
const TrackedObjectConstPtr &  new_object 
) const
protected

Check whether new observed object is static or not

参数
latest_objectlatest history object
new_objectnew object to be updated
返回
true if new object is static
false

在文件 mlf_motion_refiner.cc 第 111 行定义.

113 {
114 // Check whether track is static or not
115 // evaluate speed noise level, the less the level is the
116 // greater the probability of noise is
117 double speed = new_object->output_velocity.head(2).norm();
118
119 // TypeBasedThreshold
120 bool velocity_noise_level_0 = false;
121 bool velocity_noise_level_1 = false;
122 bool velocity_noise_level_2 = false;
123 auto type_select = new_object->type;
124 // for pedestrian we give loose threshold
125 if (type_select == base::ObjectType::PEDESTRIAN ||
126 type_select == base::ObjectType::UNKNOWN) {
127 velocity_noise_level_0 = speed < (claping_speed_threshold_ / 4.0);
128 velocity_noise_level_1 = speed < (claping_speed_threshold_ / 2.0);
129 velocity_noise_level_2 = speed < (claping_speed_threshold_ / 1.0);
130 } else if (type_select == base::ObjectType::BICYCLE) {
131 velocity_noise_level_0 = speed < (cyc_refine_speed_ / 6.0);
132 velocity_noise_level_1 = speed < (cyc_refine_speed_ / 4.0);
133 velocity_noise_level_2 = speed < (cyc_refine_speed_ / 2.0);
134 } else if (type_select == base::ObjectType::VEHICLE) {
135 velocity_noise_level_0 = speed < (car_refine_speed_ / 8.0);
136 velocity_noise_level_1 = speed < (car_refine_speed_ / 4.0);
137 velocity_noise_level_2 = speed < (car_refine_speed_ / 2.0);
138 }
139
140 // believe track is static if speed_level 0 is so small
141 if (velocity_noise_level_0) {
142 ADEBUG << "type: " << static_cast<size_t>(type_select)
143 << " speed is " << speed << " so small";
144 return true;
145 }
146
147 double velocity_change_angle = M_PI, velocity_heading_angle = M_PI;
148 if (CalculateVelocityAngleChange(latest_object, new_object,
149 &velocity_change_angle, &velocity_heading_angle)) {
150 ADEBUG << " velocity_change_angle: " << velocity_change_angle
151 << " velocity_heading_angle: " << velocity_heading_angle;
152 }
153
154 // believe track is static
155 // speed_level 1 and velocity-change or velocity-heading change > PI/6
156 bool dir_change_level_0 = (fabs(velocity_change_angle) > M_PI / 6 ||
157 fabs(velocity_heading_angle) > M_PI / 6);
158 if (velocity_noise_level_1 && dir_change_level_0) {
159 ADEBUG << "type: " << static_cast<size_t>(type_select) << " speed is "
160 << speed << " velocity_change is " << velocity_change_angle
161 << " velocity_heading is " << velocity_heading_angle
162 << " bigger than PI/6";
163 return true;
164 }
165
166 // believe track is static
167 // speed_level 2 and velocity-change or velocity-heading change is > PI/4
168 bool dir_change_level_1 = (fabs(velocity_change_angle) > M_PI / 4 ||
169 fabs(velocity_heading_angle) > M_PI / 4);
170 if (velocity_noise_level_2 && dir_change_level_1) {
171 ADEBUG << "type: " << static_cast<size_t>(type_select) << " speed is "
172 << speed << " velocity_change is " << velocity_change_angle
173 << " velocity_heading is " << velocity_heading_angle
174 << " bigger than PI/4";
175 return true;
176 }
177 return false;
178}
apollo::perception::lidar::MlfMotionRefiner::CalculateVelocityAngleChange
bool CalculateVelocityAngleChange(const TrackedObjectConstPtr &latest_object, const TrackedObjectConstPtr &new_object, double *vel_change_angle, double *velocity_heading_angle) const
Definition mlf_motion_refiner.cc:180
ADEBUG
#define ADEBUG
Definition log.h:41

◆ CheckStaticHypothesisByVelocityAngleChange()

bool apollo::perception::lidar::MlfMotionRefiner::CheckStaticHypothesisByVelocityAngleChange ( const TrackedObjectConstPtr &  latest_object,
const TrackedObjectConstPtr &  new_object,
double  reasonable_angle_change_maximum 
) const
protected

Check whether new observed object is static or not via considering the angle velocity change

参数
latest_objectlatest history object
new_objectnew object to be updated
reasonable_angle_change_maximummax allowed angle change threshold
返回
true if new object is static
false

在文件 mlf_motion_refiner.cc 第 206 行定义.

209 {
210 // note reasonable_angle_change_maximum should be within [0, M_PI]
211 // believe angle change is obvious if one of estimation pair is
212 // extrodinary small
213 Eigen::Vector3d previous_velocity = latest_object->output_velocity;
214 Eigen::Vector3d current_velocity = new_object->output_velocity;
215 constexpr double kEpsilon = std::numeric_limits<double>::epsilon();
216 if (previous_velocity.norm() < kEpsilon) {
217 // return false; // without motion score, this should be true
218 return true;
219 }
220 if (current_velocity.norm() < kEpsilon) {
221 return true;
222 }
223 // believe angle change is obvious if it is greater than threshold
224
225 double velocity_angle_change =
226 algorithm::CalculateTheta2DXY(previous_velocity, current_velocity);
227 if (fabs(velocity_angle_change) > reasonable_angle_change_maximum) {
228 return true;
229 }
230 // Let track be static if angle change of current velocity
231 // & previous heading is larger
232 // than given reasonable maximum.
233 Eigen::Vector3d previous_direction = latest_object->output_direction;
234 double velocity_heading_angle_change_1 =
235 algorithm::CalculateTheta2DXY(previous_direction, current_velocity);
236 previous_direction *= -1;
237 double velocity_heading_angle_change_2 =
238 algorithm::CalculateTheta2DXY<double>(
239 previous_direction, current_velocity);
240 velocity_angle_change = std::min(fabs(velocity_heading_angle_change_1),
241 fabs(velocity_heading_angle_change_2));
242 if (fabs(velocity_angle_change) > reasonable_angle_change_maximum) {
243 return true;
244 }
245 return false;
246}
apollo::perception::algorithm::CalculateTheta2DXY
T CalculateTheta2DXY(const Eigen::Matrix< T, 3, 1 > &v1, const Eigen::Matrix< T, 3, 1 > &v2)
Definition basic.h:92

◆ Init()

bool apollo::perception::lidar::MlfMotionRefiner::Init ( const MlfMotionRefinerInitOptions &  options = MlfMotionRefinerInitOptions())

Init motion refiner

参数
options
返回
true
false

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

30 {
31 std::string config_file = "mlf_motion_refiner.conf";
32 if (!options.config_file.empty()) {
33 config_file = options.config_file;
34 }
35 config_file = GetConfigFile(options.config_path, config_file);
36 MlfMotionRefinerConfig config;
37 ACHECK(apollo::cyber::common::GetProtoFromFile(config_file, &config));
38 // read from proto config
39 claping_speed_threshold_ = config.claping_speed_threshold();
40 claping_acceleration_threshold_ = config.claping_acceleration_threshold();
41 cyc_refine_speed_ = config.cyc_refine_speed();
42 car_refine_speed_ = config.car_refine_speed();
43 use_movable_state_check_ = config.use_movable_state_check();
44 return true;
45}
ACHECK
#define ACHECK(cond)
Definition log.h:80
apollo::cyber::common::GetProtoFromFile
bool GetProtoFromFile(const std::string &file_name, google::protobuf::Message *message)
Parses the content of the file specified by the file_name as a representation of protobufs,...
Definition file.cc:132
apollo::perception::GetConfigFile
std::string GetConfigFile(const std::string &config_path, const std::string &config_file)
Definition util.cc:80

◆ Name()

std::string apollo::perception::lidar::MlfMotionRefiner::Name ( ) const
inline

Get class name

返回
std::string

在文件 mlf_motion_refiner.h 第 65 行定义.

65{ return "MlfMotionRefiner"; }

◆ NeedAssignMovableObjectSpeed()

bool apollo::perception::lidar::MlfMotionRefiner::NeedAssignMovableObjectSpeed ( const TrackedObjectConstPtr &  latest_object,
const TrackedObjectConstPtr &  new_object 
) const
protected

在文件 mlf_motion_refiner.cc 第 248 行定义.

250 {
251 base::ObjectPtr obj = new_object->object_ptr;
252 // moving object and have no speed
253 if (obj->motion_state == base::MotionState::MOVING &&
254 new_object->output_velocity.head(2).norm() < 1e-6) {
255 return true;
256 }
257
258 return false;
259}
apollo::perception::base::ObjectPtr
std::shared_ptr< Object > ObjectPtr
Definition object.h:127

◆ Refine()

bool apollo::perception::lidar::MlfMotionRefiner::Refine ( const MlfTrackDataConstPtr &  track_data,
TrackedObjectPtr  new_object 
)

Refine velocity of new object

参数
track_datahistory track data
new_objectnew object to be updated
返回
true if velocity is changed
false

在文件 mlf_motion_refiner.cc 第 47 行定义.

48 {
49 auto latest_object_pair = track_data->GetLatestObject();
50 const TrackedObjectConstPtr& latest_object = latest_object_pair.second;
51
52 // 1. keep motion if acceleration of filter is greater than a threshold
53 double time_diff = new_object->object_ptr->latest_tracked_time -
54 latest_object->object_ptr->latest_tracked_time;
55 Eigen::Vector3d filter_velocity_gain = new_object->belief_velocity_gain;
56 double filter_acceleration_gain = 0.0;
57 if (fabs(time_diff) < EPSION_TIME) {
58 filter_acceleration_gain = 0.0;
59 } else {
60 filter_acceleration_gain = filter_velocity_gain.norm() / time_diff;
61 }
62 bool need_keep_motion =
63 filter_acceleration_gain > claping_acceleration_threshold_;
64 // use tighter threshold for pedestrian
65 if (filter_acceleration_gain > claping_acceleration_threshold_ / 2 &&
66 new_object->type == base::ObjectType::PEDESTRIAN) {
67 need_keep_motion = true;
68 }
69
70 if (need_keep_motion && (!new_object->converged)) {
71 Eigen::Vector3d current_velocity = Eigen::Vector3d::Zero();
72 current_velocity = latest_object->output_velocity;
73 new_object->output_velocity = current_velocity;
74 // new_object->output_velocity = Eigen::Vector3d::Zero();
75 // new_object->state.tail<2>().setZero();
76 // new_object->belief_velocity = Eigen::Vector3d::Zero();
77 // new_object->state.head<2>().setZero();
78 AINFO << "[Velocity-Refine] track_id: " << track_data->track_id_
79 << " KEEP MOTION because of extraodinary acceleration.";
80 return true;
81 }
82
83 // 2. motion state check
84 bool need_assign_movable_obj_speed =
85 NeedAssignMovableObjectSpeed(latest_object, new_object);
86 if (use_movable_state_check_ && need_assign_movable_obj_speed) {
87 if (new_object->belief_velocity.head(2).norm() > 0) {
88 new_object->output_velocity = new_object->belief_velocity;
89 } else if (new_object->measured_barycenter_velocity.head(2).norm() > 0) {
90 new_object->output_velocity = new_object->measured_barycenter_velocity;
91 } else {
92 new_object->output_velocity = new_object->measured_center_velocity;
93 }
94 return true;
95 }
96
97 // 3. static hypothesis check
98 bool is_static_hypothesis =
99 CheckStaticHypothesisByState(latest_object, new_object);
100
101 if (is_static_hypothesis) {
102 new_object->output_velocity = Eigen::Vector3d::Zero();
103 new_object->state.tail<2>().setZero();
104 AINFO << "[Velocity-Refine] track_id: " << track_data->track_id_
105 << " set velocity to zero because of static_hypothesis.";
106 return true;
107 }
108 return false;
109}
apollo::perception::lidar::MlfMotionRefiner::CheckStaticHypothesisByState
bool CheckStaticHypothesisByState(const TrackedObjectConstPtr &latest_object, const TrackedObjectConstPtr &new_object) const
Check whether new observed object is static or not
Definition mlf_motion_refiner.cc:111
apollo::perception::lidar::MlfMotionRefiner::NeedAssignMovableObjectSpeed
bool NeedAssignMovableObjectSpeed(const TrackedObjectConstPtr &latest_object, const TrackedObjectConstPtr &new_object) const
Definition mlf_motion_refiner.cc:248
AINFO
#define AINFO
Definition log.h:42
apollo::perception::lidar::TrackedObjectConstPtr
std::shared_ptr< const TrackedObject > TrackedObjectConstPtr
Definition tracked_object.h:231

类成员变量说明

◆ car_refine_speed_

double apollo::perception::lidar::MlfMotionRefiner::car_refine_speed_ = 1.0
protected

在文件 mlf_motion_refiner.h 第 109 行定义.

◆ claping_acceleration_threshold_

double apollo::perception::lidar::MlfMotionRefiner::claping_acceleration_threshold_ = 10
protected

在文件 mlf_motion_refiner.h 第 106 行定义.

◆ claping_speed_threshold_

double apollo::perception::lidar::MlfMotionRefiner::claping_speed_threshold_ = 1.0
protected

在文件 mlf_motion_refiner.h 第 107 行定义.

◆ cyc_refine_speed_

double apollo::perception::lidar::MlfMotionRefiner::cyc_refine_speed_ = 1.2
protected

在文件 mlf_motion_refiner.h 第 108 行定义.

◆ EPSION_TIME

const double apollo::perception::lidar::MlfMotionRefiner::EPSION_TIME = 1e-3
protected

在文件 mlf_motion_refiner.h 第 110 行定义.

◆ use_movable_state_check_

bool apollo::perception::lidar::MlfMotionRefiner::use_movable_state_check_ = false
protected

在文件 mlf_motion_refiner.h 第 111 行定义.

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