Class BayesianGoalPrediction
Defined in File predict_goal.h
Class Documentation
-
class BayesianGoalPrediction
Class implementing Bayesian goal prediction for agents.
Public Types
-
using Trajectory = std::vector<Eigen::Vector2d>
Public Functions
-
inline BayesianGoalPrediction()
Constructor initializing with default covariance.
-
~BayesianGoalPrediction() = default
Default destructor.
-
inline void initialize(const std::map<std::string, Eigen::Vector2d> &goals, int window_size)
Initialize the predictor with potential goals and window size.
- Parameters:
goals – Map of goal names to their 2D positions
window_size – Size of the sliding window for trajectory analysis
-
inline std::string predictGoal(int id, Eigen::Vector2d &xy)
Predict the most likely goal for an agent.
- Parameters:
id – Agent identifier
xy – Current position of the agent
- Returns:
Name of the predicted goal, “None” if insufficient data
Private Functions
-
inline void addPosition(int id, Eigen::Vector2d &xy)
Add a new position to agent’s trajectory. This method updates the trajectory of the agent by adding the current position. If the trajectory exceeds the window size, the oldest position is removed.
- Parameters:
id – Agent ID
xy – Current position of the agent as a 2D vector
-
inline void getProbabilities(int id)
Calculate probabilities for each goal based on the agent’s trajectory. This method computes the likelihood of each goal being the agent’s target based on the agent’s motion history and predefined goal priors.
- Parameters:
id – Agent ID
Private Members
-
std::vector<Eigen::Vector2d> goals_
List of goal positions.
-
std::vector<std::string> goal_names_
List of goal names.
-
std::map<int, Trajectory> agents_trajs_
Map of agent trajectories.
-
std::map<int, std::vector<double>> goal_priors_
Map of goal priors for each agent.
-
std::map<int, std::vector<double>> agent_probs_
Map of probabilities for each agent.
-
int window_size_
Sliding window size for trajectory analysis.
-
using Trajectory = std::vector<Eigen::Vector2d>