53 double timestamp = obj->timestamp;
55 auto pair = std::make_pair(timestamp, obj);
81 AINFO <<
"Push object timestamp " << timestamp <<
" from sensor "
82 << obj->sensor_info.name <<
" already exist in track, ignore push.";
87 double timestamp = obj->object_ptr->latest_tracked_time;
92 AINFO <<
"Push object timestamp " << timestamp <<
" from sensor "
93 << obj->sensor_info.name <<
" already exist in cache, ignore push.";
103 const double latest_time = latest_iter->first;
104 const auto& latest_object = latest_iter->second;
105 latest_object->ToObject(
object);
107 double time_diff = timestamp - latest_time;
108 double offset_x = time_diff *
object->velocity(0) + local_to_global_offset(0);
109 double offset_y = time_diff *
object->velocity(1) + local_to_global_offset(1);
110 double offset_z = time_diff *
object->velocity(2) + local_to_global_offset(2);
112 for (
auto& pt :
object->polygon) {
119 object->center(0) += offset_x;
120 object->center(1) += offset_y;
121 object->center(2) += offset_z;
123 object->anchor_point(0) += offset_x;
124 object->anchor_point(1) += offset_y;
125 object->anchor_point(2) += offset_z;
128 object->tracking_time += time_diff;
130 object->latest_tracked_time += time_diff;
133 for (
auto& pt :
object->lidar_supplement.cloud_world) {
146 const double latest_time = latest_iter->first;
147 const auto& latest_object = latest_iter->second;
148 double time_diff = timestamp - latest_time;
150 const Eigen::Vector3d& latest_anchor_point =
151 latest_object->belief_anchor_point;
152 const Eigen::Vector3d& latest_velocity = latest_object->output_velocity;
155 predict_.
state(0) =
static_cast<float>(latest_anchor_point(0) +
156 latest_velocity(0) * time_diff);
157 predict_.
state(1) =
static_cast<float>(latest_anchor_point(1) +
158 latest_velocity(1) * time_diff);
159 predict_.
state(2) =
static_cast<float>(latest_anchor_point(2) +
160 latest_velocity(2) * time_diff);
170 std::vector<TrackedObjectPtr>* objects) {
174 const auto& timestamp = iter->first;
178 objects->push_back(iter->second);
187 std::map<double, TrackedObjectPtr>* data) {
188 auto iter = data->begin();
189 while (iter != data->end()) {
190 if (iter->first < timestamp) {
206 std::vector<TrackedObjectPtr>* objects) {
212 objects->push_back(obj_it->second);
221 std::vector<TrackedObjectConstPtr>* objects)
const {
227 objects->push_back(obj_it->second);
236 std::vector<TrackedObjectPtr>* objects) {
240 if (obj_it->second->timestamp >= time) {
241 objects->push_back(obj_it->second);
249 std::vector<TrackedObjectConstPtr>* objects)
const {
253 if (obj_it->second->timestamp >= time) {
254 objects->push_back(obj_it->second);
262 std::vector<TrackedObjectConstPtr>* objects) {
265 if (obj_it->second->timestamp >= time) {
266 objects->push_back(obj_it->second);
272 std::vector<TrackedObjectConstPtr>* objects)
const {
275 if (obj_it->second->timestamp >= time) {
276 objects->push_back(obj_it->second);
284 obj->object_ptr->lidar_supplement.is_clustered ? 0.0 : 1.0;
287 obj->object_ptr->lidar_supplement.is_background =
false;
288 obj->is_background =
false;
290 if (!obj->object_ptr->lidar_supplement.is_clustered) {
303 bool is_background_movable =
304 (obj->object_ptr->lidar_supplement.dynamic_state ==
306 (obj->object_ptr->lidar_supplement.semantic_type ==
309 return is_background_movable;
bool IsBackgroundMovable(TrackedObjectPtr obj)
void GetObjectsInIntervalByOrder(double time, std::vector< TrackedObjectConstPtr > *objects)
double latest_visible_time_
static const double kMaxHistoryTime
double consecutive_invisible_time_
TimedObjects cached_objects_
void PushTrackedObjectToTrack(TrackedObjectPtr obj)
Push tracked object to track
void RemoveStaleHistory(double timestamp)
Remove stale history data
bool is_reserve_blind_cone_
void GetLatestKObjects(size_t k, std::vector< TrackedObjectPtr > *objects)
void GetObjectsInInterval(double time, std::vector< TrackedObjectPtr > *objects)
bool is_front_critical_track_
bool IsForegroundTrack() const
bool is_current_state_predicted_
void UpdateTrackableState(TrackedObjectPtr obj)
float foreground_track_prob_
void PredictState(double timestamp) const
Predict state
std::map< std::string, TimedObjects > sensor_history_objects_
bool ToObject(const Eigen::Vector3d &local_to_global_offset, double timestamp, base::ObjectPtr object, bool update_time=true) const
Convert tracked object to base object
void PushTrackedObjectToCache(TrackedObjectPtr obj)
Push tracked object to cache
void Reset() override
Reset
double first_tracked_time_
void GetAndCleanCachedObjectsInTimeInterval(std::vector< TrackedObjectPtr > *objects)
Get and clean cached objects in time interval
double latest_cached_time_
int consecutive_invisible_count_
std::map< double, TrackedObjectPtr > history_objects_
virtual void Reset()
Reset
std::shared_ptr< Object > ObjectPtr
void RemoveStaleDataFromMap(double timestamp, std::map< double, TrackedObjectPtr > *data)
std::shared_ptr< TrackedObject > TrackedObjectPtr
void Reset()
Reset all data