double EnvelopeScore::score(const std::vector<IMP::algebra::Vector3D>& points,
const IMP::algebra::Transformation3D& trans) const {
std::vector<IMP::algebra::Vector3D> transformed_points(points.size());
for (unsigned int i = 0; i < points.size(); i++)
transformed_points[i] = trans * points[i];
return score(transformed_points);
}
Eigen::MatrixXd ObjectsToCostmap::makeRectanglePoints(const autoware_msgs::DetectedObject& in_object,
const double expand_rectangle_size)
{
double length = in_object.dimensions.x + expand_rectangle_size;
double width = in_object.dimensions.y + expand_rectangle_size;
Eigen::MatrixXd origin_points(NUMBER_OF_DIMENSIONS, NUMBER_OF_POINTS);
origin_points << length / 2, length / 2, -length / 2, -length / 2, width / 2, -width / 2, -width / 2, width / 2;
double yaw = tf::getYaw(in_object.pose.orientation);
Eigen::MatrixXd rotation_matrix(NUMBER_OF_DIMENSIONS, NUMBER_OF_DIMENSIONS);
rotation_matrix << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw);
Eigen::MatrixXd rotated_points = rotation_matrix * origin_points;
double dx = in_object.pose.position.x;
double dy = in_object.pose.position.y;
Eigen::MatrixXd transformed_points(NUMBER_OF_DIMENSIONS, NUMBER_OF_POINTS);
Eigen::MatrixXd ones = Eigen::MatrixXd::Ones(1, NUMBER_OF_POINTS);
transformed_points.row(0) = rotated_points.row(0) + dx * ones;
transformed_points.row(1) = rotated_points.row(1) + dy * ones;
return transformed_points;
}
void addCloud (OverlayClouds* overlay, LocalizedCloud::ConstPtr cloud, const int inc)
{
ROS_ASSERT_MSG(overlay->frame_id==cloud->header.frame_id,
"Frame id %s of overlayed cloud didn't match existing one %s",
cloud->header.frame_id.c_str(), overlay->frame_id.c_str());
ROS_ASSERT(overlay->grid.info.width*overlay->grid.info.height > 0);
const gm::Point& sensor_pos = cloud->sensor_pose.position;
ROS_DEBUG_NAMED ("overlay", "Ray tracing from %.2f, %.2f", sensor_pos.x, sensor_pos.y);
// Transform points to world frame
tf::Pose sensor_to_world;
tf::poseMsgToTF(cloud->sensor_pose, sensor_to_world);
vector<gm::Point> transformed_points(cloud->cloud.points.size());
transform(cloud->cloud.points.begin(),
cloud->cloud.points.end(),
transformed_points.begin(),
bind(transformPt, ref(sensor_to_world), _1));
// Iterate over points in this cloud
BOOST_FOREACH (const gm::Point& p, transformed_points) {
ROS_DEBUG_NAMED ("overlay_counts", " Ray tracing to point %.2f, %.2f", p.x, p.y);
boost::optional<index_t> last_ind;
const bool have_existing_grid = !overlay->grid.data.empty();
// Inner loop: raytrace along the line and update counts
// We allow both the sensor pose and the target to be off the grid
BOOST_FOREACH (const Cell& c, rayTrace(overlay->grid.info, sensor_pos, p, true, true, overlay->max_distance)) {
last_ind = cellIndex(overlay->grid.info, c);
overlay->pass_through_counts[*last_ind] += inc;
if (have_existing_grid && overlay->grid.data[*last_ind] == OCCUPIED)
break;
ROS_ASSERT(overlay->pass_through_counts[*last_ind]>=0);
ROS_DEBUG_NAMED ("overlay_counts", " Pass-through counts for %d, %d are now %u", c.x, c.y,
overlay->pass_through_counts[*last_ind]);
}
if (last_ind) {
// If the last cell equals the point (i.e., point is not off the grid), update hit counts
const Cell last_cell = indexCell(overlay->grid.info, *last_ind);
if (last_cell == pointCell(overlay->grid.info, p)) {
#ifdef GRID_UTILS_GCC_46
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wuninitialized"
#endif
overlay->hit_counts[*last_ind] += inc;
#ifdef GRID_UTILS_GCC_46
#pragma GCC diagnostic pop
#endif
ROS_ASSERT(overlay->hit_counts[*last_ind]>=0);
ROS_DEBUG_NAMED ("overlay_counts", " Hit counts for %d, %d are now %u", last_cell.x, last_cell.y,
overlay->hit_counts[*last_ind]);
}
}
}
