逐行详细解释: void DstExistenceFusion::UpdateWithoutMeasurement(const std::string &sensor_id, double measurement_timestamp, double target_timestamp, double min_match_dist) { SensorObjectConstPtr camera_object = nullptr; if (common::SensorManager::Instance()->IsCamera(sensor_id)) { camera_object = track_ref_->GetSensorObject(sensor_id); UpdateToicWithoutCameraMeasurement(sensor_id, measurement_timestamp, min_match_dist); } SensorObjectConstPtr lidar_object = track_ref_->GetLatestLidarObject(); SensorObjectConstPtr camera_object_latest = track_ref_->GetLatestCameraObject(); SensorObjectConstPtr radar_object = track_ref_->GetLatestRadarObject(); if ((lidar_object != nullptr && lidar_object->GetSensorId() == sensor_id) || (camera_object_latest != nullptr && camera_object_latest->GetSensorId() == sensor_id) || (radar_object != nullptr && radar_object->GetSensorId() == sensor_id && lidar_object == nullptr && camera_object_latest == nullptr)) { Dst existence_evidence(fused_existence_.Name()); double unexist_factor = GetUnexistReliability(sensor_id); base::ObjectConstPtr obj = track_ref_->GetFusedObject()->GetBaseObject(); double dist_decay = ComputeDistDecay(obj, sensor_id, measurement_timestamp); double obj_unexist_prob = unexist_factor * dist_decay; existence_evidence.SetBba( {{ExistenceDstMaps::NEXIST, obj_unexist_prob}, {ExistenceDstMaps::EXISTUNKNOWN, 1 - obj_unexist_prob}}); // TODO(all) hard code for fused exist bba const double unexist_fused_w = 1.0; double min_match_dist_score = min_match_dist; // if (!sensor_manager->IsCamera(sensor_id)) { // min_match_dist_score = std::max(1 - min_match_dist / // options_.track_object_max_match_distance_, 0.0); // } ADEBUG << " before update exist prob: " << GetExistenceProbability() << " min_match_dist: " << min_match_dist << " min_match_dist_score: " << min_match_dist_score; fused_existence_ = fused_existence_ + existence_evidence * unexist_fused_w * (1 - min_match_dist_score); ADEBUG << " update without, EXIST prob: " << GetExistenceProbability() << " 1 - match_dist_score: " << 1 - min_match_dist_score << " sensor_id: " << sensor_id << " dist_decay: " << dist_decay << " track_id: " << track_ref_->GetTrackId(); } UpdateExistenceState(); }
时间: 2024-04-28 11:23:36 浏览: 152
这是一个名为DstExistenceFusion的类中的一个函数UpdateWithoutMeasurement。该函数的作用是在没有测量数据的情况下更新目标的存在状态。该函数接受四个参数,分别为传感器ID(sensor_id)、测量时间戳(measurement_timestamp)、目标时间戳(target_timestamp)和最小匹配距离(min_match_dist)。
首先,该函数通过传感器ID获取摄像头的SensorObject(SensorObjectConstPtr类型)。
然后,通过调用track_ref_对象的GetLatestLidarObject()、GetLatestCameraObject()和GetLatestRadarObject()函数,获取最新的激光雷达、摄像头和毫米波雷达的SensorObject(SensorObjectConstPtr类型)。
接下来,如果激光雷达、摄像头或毫米波雷达的SensorObject的SensorId与传入的sensor_id相同,则执行目标存在性融合操作。在目标存在性融合操作中,首先创建一个Dst类型的存在证据(existence_evidence),并设置其置信度。置信度的计算包括了未存在因子(unexist_factor)、距离衰减因子(dist_decay)和目标不存在的概率(obj_unexist_prob)。
然后,将existence_evidence与融合目标的存在证据(fused_existence_)进行合并,得到新的存在证据。在合并操作中,需要考虑到未存在因子的权重和最小匹配距离的得分。
最后,更新目标的存在状态并返回。
阅读全文