| 91 | } |
| 92 | |
| 93 | double ArucoSensorModel::ComputeLogProb(const Particle & particle) |
| 94 | { |
| 95 | double log_prob = 0; |
| 96 | for (const stsl_interfaces::msg::Tag & tag : last_msg_.tags) { |
| 97 | TagLocation body_location; |
| 98 | // ensure this is a localization tag |
| 99 | if (tags_.find(tag.id) == tags_.end()) { |
| 100 | continue; |
| 101 | } |
| 102 | // convert the global location of the current tag id into body frame |
| 103 | TagLocation map_location = tags_.at(tag.id); |
| 104 | double x_diff = map_location.x - particle.x; |
| 105 | double y_diff = map_location.y - particle.y; |
| 106 | |
| 107 | // rotation matrix is transposed |
| 108 | body_location.x = x_diff * cos(particle.yaw) - y_diff * sin(particle.yaw); |
| 109 | body_location.y = x_diff * sin(particle.yaw) + y_diff * cos(particle.yaw); |
| 110 | |
| 111 | double expected_dist = sqrt(pow(body_location.x, 2) + pow(body_location.y, 2) + pow(0.05, 2)); |
| 112 | double dist = |
| 113 | sqrt( |
| 114 | pow( |
| 115 | tag.pose.position.x, |
| 116 | 2) + pow(tag.pose.position.y, 2) + pow(tag.pose.position.z, 2)); |
| 117 | |
| 118 | log_prob += pow(expected_dist - dist, 2) / covariance_[0]; |
| 119 | |
| 120 | double bearing = atan2(tag.pose.position.y, tag.pose.position.x); |
| 121 | double expected_bearing = atan2(body_location.y, body_location.x); |
| 122 | |
| 123 | const auto angular_error = angles::shortest_angular_distance(expected_bearing, bearing); |
| 124 | log_prob += pow(angular_error, 2) / covariance_[1]; |
| 125 | } |
| 126 | return log_prob; |
| 127 | } |
| 128 | |
| 129 | bool ArucoSensorModel::IsMeasurementAvailable(const rclcpp::Time & cur_time) |
| 130 | { |
no outgoing calls
no test coverage detected