mrf_motion_filter.h

浏览该文件的文档.

/******************************************************************************
 * Copyright 2023 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#pragma once
 
#include <memory>
#include <string>
 
#include "modules/perception/radar4d_detection/lib/tracker/multi_radar_fusion/mrf_base_filter.h"
#include "modules/perception/radar4d_detection/lib/tracker/multi_radar_fusion/mrf_motion_measurement.h"
#include "modules/perception/radar4d_detection/lib/tracker/multi_radar_fusion/mrf_motion_refiner.h"
 
namespace apollo {
namespace perception {
namespace radar4d {
 
class MrfMotionFilter : public MrfBaseFilter {
 public:
  MrfMotionFilter() = default;
  virtual ~MrfMotionFilter() = default;
 
  bool Init(
      const MrfFilterInitOptions& options = MrfFilterInitOptions()) override;
 
  void UpdateWithObject(const MrfFilterOptions& options,
                        const MrfTrackDataConstPtr& track_data,
                        TrackedObjectPtr new_object) override;
 
  void UpdateWithoutObject(const MrfFilterOptions& options, double timestamp,
                           MrfTrackDataPtr track_data) override;
 
  std::string Name() const override { return "MrfMotionFilter"; }
 
 protected:
  void InitializeTrackState(TrackedObjectPtr new_object);
 
  void KalmanFilterUpdateWithPartialObservation(
      const MrfTrackDataConstPtr& track_data,
      const TrackedObjectConstPtr& latest_object, TrackedObjectPtr new_object);
 
  void StateGainAdjustment(const MrfTrackDataConstPtr& track_data,
                           const TrackedObjectConstPtr& latest_object,
                           const TrackedObjectConstPtr& new_object,
                           Eigen::Vector4d* gain);
 
  void ConvergenceEstimationAndBoostUp(
      const MrfTrackDataConstPtr& track_data,
      const TrackedObjectConstPtr& latest_object, TrackedObjectPtr new_object);
 
  void ComputeConvergenceConfidence(const MrfTrackDataConstPtr& track_data,
                                    TrackedObjectPtr new_object,
                                    bool velocity_source_is_belief = true);
 
  void BoostupState(const MrfTrackDataConstPtr& track_data,
                    TrackedObjectPtr new_object);
 
  void ClipingState(TrackedObjectPtr object);
 
  void OnlineCovarianceEstimation(const MrfTrackDataConstPtr& track_data,
                                  TrackedObjectPtr object);
 
  void UpdateConverged(const MrfTrackDataConstPtr& track_data,
                       TrackedObjectPtr object);
 
  void StateToBelief(TrackedObjectPtr object);
 
  void BeliefToOutput(TrackedObjectPtr object);
 
 protected:
  const double EPSION_TIME = 1e-3;
  const double DEFAULT_FPS = 0.1;
  // motion measurement
  std::shared_ptr<MrfMotionMeasurement> motion_measurer_;
  // motion refiner
  std::shared_ptr<MrfMotionRefiner> motion_refiner_;
  // switch for filter strategies
  bool use_adaptive_ = true;
  bool use_breakdown_ = true;
  bool use_convergence_boostup_ = true;
  // default covariance parameters for kalman filter
  double init_velocity_variance_ = 5.0;
  double init_acceleration_variance_ = 10.0;
  double measured_velocity_variance_ = 0.4;
  double predict_variance_per_sqrsec_ = 50.0;
  // other parameters
  size_t boostup_history_size_minimum_ = 3;
  size_t boostup_history_size_maximum_ = 6;
  double converged_confidence_minimum_ = 0.5;
  double noise_maximum_ = 0.1;
  double trust_orientation_range_ = 40.0;
};  // class MrfMotionFilter
 
}  // namespace radar4d
}  // namespace perception
}  // namespace apollo