Skip to content

added pedestrian prediction #344

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
girish316 wants to merge 1 commit into
base: main
Choose a base branch
from
Open

added pedestrian prediction #344

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
158 changes: 133 additions & 25 deletions src/world_modeling/prediction/src/trajectory_predictor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -246,44 +246,152 @@ std::vector<TrajectoryHypothesis> TrajectoryPredictor::generateVehicleHypotheses
return hypotheses;
}

std::vector<TrajectoryHypothesis> TrajectoryPredictor::generatePedestrianHypotheses(
std::vector<TrajectoryHypothesis>TrajectoryPredictor::generatePedestrianHypotheses(
const vision_msgs::msg::Detection3D & detection)
{
std::vector<TrajectoryHypothesis> hypotheses;
hypotheses.reserve(3); // small optimization

// Guard against invalid configuration
if (time_step_ <= 0.0) {
RCLCPP_ERROR(node_->get_logger(), "Prediction time_step must be > 0");
return hypotheses;
}

// Current ROS time for trajectory timestamps
rclcpp::Time current_time = node_->get_clock()->now();
std::string frame_id = "map";
std::string frame_id = "map"; // must match detection frame upstream

// Extract pedestrian state
const double start_x = detection.bbox.center.position.x;
const double start_y = detection.bbox.center.position.y;
const double start_z = detection.bbox.center.position.z;

// Heading is noisy, but still useful as a nominal direction
const double heading_yaw = extractYaw(detection.bbox.center.orientation);

// Typical pedestrian walking speed (m/s)
const double nominal_walk_speed = 1.4;

// Precompute orientation aligned with heading (planner-friendly)
geometry_msgs::msg::Quaternion aligned_orientation;
aligned_orientation.w = std::cos(heading_yaw * 0.5);
aligned_orientation.z = std::sin(heading_yaw * 0.5);

// Hypothesis 1: Confident forward walking
TrajectoryHypothesis confident_walk;
Copy link
Contributor

@RyanL2 RyanL2 Feb 11, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

generatePedestrianHypotheses() sets probability (0.55 / 0.30 / 0.15), but processObject() in prediction_node.cpp:159-160 then calls intent_classifier_->assignProbabilities() which overwrites them.

I was planning for us to not use the intent classifier yet. But since vehicle prediction already used intent classifier, let's refactor the assigning probabilities part to the intent classifier for pedestrians for consistency.

confident_walk.header.stamp = current_time;
confident_walk.header.frame_id = frame_id;
confident_walk.intent = Intent::CONTINUE_STRAIGHT;
confident_walk.probability = 0.55;

for (double time_sec = time_step_;
time_sec <= prediction_horizon_;
time_sec += time_step_) {

geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = frame_id;
pose.header.stamp = current_time + rclcpp::Duration::from_seconds(time_sec);

// Constant-velocity forward propagation
pose.pose.position.x =
start_x + nominal_walk_speed * std::cos(heading_yaw) * time_sec;
pose.pose.position.y =
start_y + nominal_walk_speed * std::sin(heading_yaw) * time_sec;
pose.pose.position.z = start_z;

// Align orientation with direction of travel
pose.pose.orientation = aligned_orientation;

confident_walk.poses.push_back(pose);
}

TrajectoryHypothesis walk_hyp;
walk_hyp.header.stamp = current_time;
walk_hyp.header.frame_id = frame_id;
walk_hyp.intent = Intent::CONTINUE_STRAIGHT;
walk_hyp.probability = 0.0;
hypotheses.push_back(confident_walk);

double current_x = detection.bbox.center.position.x;
double current_y = detection.bbox.center.position.y;
double current_z = detection.bbox.center.position.z;
double heading = extractYaw(detection.bbox.center.orientation);
// Hypothesis 2: Hesitant / wandering behavior
// Models uncertainty without lateral acceleration
TrajectoryHypothesis hesitant_walk;
hesitant_walk.header.stamp = current_time;
hesitant_walk.header.frame_id = frame_id;
hesitant_walk.intent = Intent::UNKNOWN;
hesitant_walk.probability = 0.30;

// Estimate speed from bounding box size heuristic
double length = detection.bbox.size.x;
double velocity = (length > 3.5) ? 5.0 : 1.4;
double accumulated_forward_distance = 0.0;

for (double t = time_step_; t <= prediction_horizon_; t += time_step_) {
geometry_msgs::msg::PoseStamped pose_stamped;
pose_stamped.header.frame_id = frame_id;
pose_stamped.header.stamp = current_time + rclcpp::Duration::from_seconds(t);
pose_stamped.pose.position.x = current_x + velocity * std::cos(heading) * t;
pose_stamped.pose.position.y = current_y + velocity * std::sin(heading) * t;
pose_stamped.pose.position.z = current_z;
pose_stamped.pose.orientation = detection.bbox.center.orientation;
for (double time_sec = time_step_;
time_sec <= prediction_horizon_;
time_sec += time_step_) {

// Slower forward progress due to hesitation
accumulated_forward_distance +=
0.6 * nominal_walk_speed * time_step_;

// Lateral uncertainty grows with time (non-cumulative)
double lateral_offset = 0.15 * std::sqrt(time_sec);
Copy link
Contributor

@RyanL2 RyanL2 Feb 11, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The hesitant walk just drifts to one side. 0.15 * sqrt(t) is always positive and only applied to one side. This looks like it's trying to model uncertainty (sqrt(t) growth) but it's just a single curved path. Add a mirrored right-side hypothesis or rethink how you want to represent the uncertainty here.


geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = frame_id;
pose.header.stamp = current_time + rclcpp::Duration::from_seconds(time_sec);

// Base forward motion
pose.pose.position.x =
start_x + accumulated_forward_distance * std::cos(heading_yaw);
pose.pose.position.y =
start_y + accumulated_forward_distance * std::sin(heading_yaw);

walk_hyp.poses.push_back(pose_stamped);
// Apply lateral deviation relative to forward path
pose.pose.position.x +=
lateral_offset * std::cos(heading_yaw + M_PI / 2.0);
pose.pose.position.y +=
lateral_offset * std::sin(heading_yaw + M_PI / 2.0);

pose.pose.position.z = start_z;
pose.pose.orientation = aligned_orientation;

hesitant_walk.poses.push_back(pose);
}

hypotheses.push_back(walk_hyp);
hypotheses.push_back(hesitant_walk);

// Hypothesis 3: Stop / yield
TrajectoryHypothesis stop_behavior;
stop_behavior.header.stamp = current_time;
stop_behavior.header.frame_id = frame_id;
stop_behavior.intent = Intent::STOP;
stop_behavior.probability = 0.15;

for (double time_sec = time_step_;
time_sec <= prediction_horizon_;
time_sec += time_step_) {

geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = frame_id;
pose.header.stamp = current_time + rclcpp::Duration::from_seconds(time_sec);

// Pedestrian remains stationary
pose.pose.position.x = start_x;
pose.pose.position.y = start_y;
pose.pose.position.z = start_z;
pose.pose.orientation = aligned_orientation;

stop_behavior.poses.push_back(pose);
}

hypotheses.push_back(stop_behavior);

// Normalize probabilities
double probability_sum = 0.0;
for (const auto & hypothesis : hypotheses) {
probability_sum += hypothesis.probability;
}
for (auto & hypothesis : hypotheses) {
hypothesis.probability /= probability_sum;
}

RCLCPP_DEBUG_ONCE(node_->get_logger(), "Pedestrian prediction: constant velocity");
RCLCPP_DEBUG(
node_->get_logger(),
"Generated %zu pedestrian trajectory hypotheses (bug-fixed, behavior-aware)",
hypotheses.size());

return hypotheses;
}
Expand Down
Loading