std::array<Vec2r, 4> Grid2dUtility::corners(int pixelId) const
{
Vec2r offset = gridToWorld(pixelId);
std::array<Vec2r, 4> corners;
corners[0] = offset;
corners[1] = offset+Vec2r(h,0);
corners[2] = offset+Vec2r(h,h);
corners[3] = offset+Vec2r(0,h);
return corners;
}
void StompCollisionSpace::getVoxelsInBody(const bodies::Body &body, std::vector<tf::Vector3> &voxels)
{
bodies::BoundingSphere bounding_sphere;
body.computeBoundingSphere(bounding_sphere);
int x,y,z,x_min,x_max,y_min,y_max,z_min,z_max;
double xw,yw,zw;
tf::Vector3 v;
worldToGrid(bounding_sphere.center,bounding_sphere.center.x()-bounding_sphere.radius,bounding_sphere.center.y()-bounding_sphere.radius,
bounding_sphere.center.z()-bounding_sphere.radius, x_min,y_min,z_min);
worldToGrid(bounding_sphere.center,bounding_sphere.center.x()+bounding_sphere.radius,bounding_sphere.center.y()+bounding_sphere.radius,
bounding_sphere.center.z()+bounding_sphere.radius, x_max,y_max,z_max);
for(x = x_min; x <= x_max; ++x)
{
for(y = y_min; y <= y_max; ++y)
{
for(z = z_min; z <= z_max; ++z)
{
gridToWorld(bounding_sphere.center,x,y,z,xw,yw,zw);
v.setX(xw);
v.setY(yw);
v.setZ(zw);
// compute all intersections
int count=0;
std::vector<tf::Vector3> pts;
body.intersectsRay(v, tf::Vector3(0, 0, 1), &pts, count);
// if we have an odd number of intersections, we are inside
if (pts.size() % 2 == 1)
voxels.push_back(v);
}
}
}
// ROS_INFO("number of occupied voxels in bounding sphere: %i", voxels.size());
}
void OccupancyGrid::getOccupiedVoxels(double x_center, double y_center, double z_center, double radius, std::string text, std::vector<geometry_msgs::Point> &voxels)
{
int x_c, y_c, z_c, radius_c;
geometry_msgs::Point v;
worldToGrid(x_center, y_center, z_center, x_c, y_c, z_c);
radius_c = radius/getResolution() + 0.5;
for(int z = z_c-radius_c; z < z_c+radius_c; ++z)
{
for(int y = y_c-radius_c; y < y_c+radius_c; ++y)
{
for(int x = x_c-radius_c; x < x_c+radius_c; ++x)
{
if(getCell(x,y,z) == 0)
{
gridToWorld(x, y, z, v.x, v.y, v.z);
voxels.push_back(v);
}
}
}
}
}
void ObstacleCombinator::generateObstacleFromCurrentCluster(std::vector<ObstacleModel::Obstacle>& obstacles)
{
Vector2<int>& c = currentCluster.cells[0];
int xMin(c.x);
int yMin(c.y);
int xMax(c.x);
int yMax(c.y);
float rightAngle = 10.0;
float leftAngle = -10.0;
Vector2<int> rightCorner;
Vector2<int> leftCorner;
Vector2<> centerCells;
Vector2<int> robotPosition(USObstacleGrid::GRID_LENGTH / 2, USObstacleGrid::GRID_LENGTH / 2);
Vector2<int> closestPoint(USObstacleGrid::GRID_LENGTH, USObstacleGrid::GRID_LENGTH);
int closestPointSqrDist(USObstacleGrid::GRID_SIZE * 2);
for(unsigned int i = 0; i < currentCluster.cells.size(); ++i)
{
c = currentCluster.cells[i];
centerCells.x += c.x;
centerCells.y += c.y;
Vector2<int> point(c.x - robotPosition.x, c.y - robotPosition.y);
int sqrDistToRobot = sqr(point.x) + sqr(point.y);
float angleToRobot = atan2(static_cast<float>(point.y), static_cast<float>(point.x));
if(angleToRobot < rightAngle)
{
rightAngle = angleToRobot;
rightCorner = Vector2<int>(c.x, c.y);
}
if(angleToRobot > leftAngle)
{
leftAngle = angleToRobot;
leftCorner = Vector2<int>(c.x, c.y);
}
if(sqrDistToRobot < closestPointSqrDist)
{
closestPoint = c;
closestPointSqrDist = sqrDistToRobot;
}
if(c.x < xMin)
xMin = c.x;
else if(c.x > xMax)
xMax = c.x;
if(c.y < yMin)
yMin = c.y;
else if(c.y > yMax)
yMax = c.y;
}
centerCells /= static_cast<float>(currentCluster.cells.size());
const float angleToCenterPoint = Geometry::angleTo(Pose2D(USObstacleGrid::GRID_LENGTH / 2, USObstacleGrid::GRID_LENGTH / 2), centerCells); //calculates the angle to the center of the cluster in grid coordinate system (independent of robot rotation)
const float cosinus = cos(-angleToCenterPoint);
const float sinus = sin(-angleToCenterPoint);
float newX;
float newY;
//initializing of the rectangle
float xMinRotated(FLT_MAX);
float yMinRotated(FLT_MAX);
float xMaxRotated(-FLT_MAX);
float yMaxRotated(-FLT_MAX);
for(unsigned int i = 0; i < currentCluster.cells.size(); ++i)
{
newX = cosinus * currentCluster.cells[i].x - sinus * currentCluster.cells[i].y; // rotates each cell of the cluster
newY = sinus * currentCluster.cells[i].x + cosinus * currentCluster.cells[i].y;
//sets new values for rectangle
if(newX < xMinRotated)
xMinRotated = newX;
else if(newX > xMaxRotated)
xMaxRotated = newX;
if(newY < yMinRotated)
yMinRotated = newY;
else if(newY > yMaxRotated)
yMaxRotated = newY;
}
Vector2<> closestPointWorld = gridToWorld(Vector2<>(closestPoint.x + 0.5f, closestPoint.y + 0.5f));
Vector2<> centerWorld = gridToWorld(centerCells);
//expansion (length of x- and y-axis through the center point) and orientation (dependent on robot rotation) of the cluster
Vector3<> covarianceEllipse(((xMaxRotated - xMinRotated) * USObstacleGrid::CELL_SIZE) * parameters.covarianceFactor, ((yMaxRotated - yMinRotated) * USObstacleGrid::CELL_SIZE) * parameters.covarianceFactor, atan2(centerWorld.y, centerWorld.x));
Matrix2x2<> covariance(covarianceEllipse.x, 0, 0, covarianceEllipse.y); // covariance is initialised with uncorrelated values (only expansion of cluster as variances)
rotateMatrix(covariance, covarianceEllipse.z); // rotates the covariance so that it fits to the orientation and expansion of the cluster
obstacles.push_back(ObstacleModel::Obstacle(gridToWorld(Vector2<>((float)(leftCorner.x), (float)(leftCorner.y))),
gridToWorld(Vector2<>((float)(rightCorner.x), (float)(rightCorner.y))), centerWorld, closestPointWorld,
static_cast<int>(currentCluster.cells.size()), covariance));
}
SReal Grid3dUtility::length(int cellId1, int cellId2) const
{
Vec3f wCoord1 = gridToWorld(cellId1);
Vec3f wCoord2 = gridToWorld(cellId2);
return (wCoord1-wCoord2).length();
}
Vec3f Grid3dUtility::gridToWorld(const int i) const
{
Vec3i gCoord = gridCoord(i);
Vec3f wCoord = gridToWorld(gCoord);
return wCoord;
}
Vec2r Grid2dUtility::gridToWorld(const int i) const
{
Vec2i gCoord = gridCoord(i);
Vec2r wCoord = gridToWorld(gCoord);
return wCoord;
}
void collision_detection::StaticDistanceField::initialize(
const bodies::Body& body,
double resolution,
double space_around_body,
bool save_points)
{
points_.clear();
inv_twice_resolution_ = 1.0 / (2.0 * resolution);
logInform(" create points at res=%f",resolution);
EigenSTL::vector_Vector3d points;
determineCollisionPoints(body, resolution, points);
if (points.empty())
{
logWarn(" StaticDistanceField::initialize: No points in body. Using origin.");
points.push_back(body.getPose().translation());
if (body.getType() == shapes::MESH)
{
const bodies::ConvexMesh& mesh = dynamic_cast<const bodies::ConvexMesh&>(body);
const EigenSTL::vector_Vector3d& verts = mesh.getVertices();
logWarn(" StaticDistanceField::initialize: also using %d vertices.", int(verts.size()));
EigenSTL::vector_Vector3d::const_iterator it = verts.begin();
EigenSTL::vector_Vector3d::const_iterator it_end = verts.end();
for ( ; it != it_end ; ++it)
{
points.push_back(*it);
}
}
}
logInform(" StaticDistanceField::initialize: Using %d points.", points.size());
AABB aabb;
aabb.add(points);
logInform(" space_around_body = %f",space_around_body);
logInform(" DF: min=(%7.3f %7.3f %7.3f) max=(%7.3f %7.3f %7.3f) (pre-space)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
aabb.max_.x(),
aabb.max_.y(),
aabb.max_.z());
aabb.min_ -= Eigen::Vector3d(space_around_body, space_around_body, space_around_body);
aabb.max_ += Eigen::Vector3d(space_around_body, space_around_body, space_around_body);
logInform(" DF: min=(%7.3f %7.3f %7.3f) max=(%7.3f %7.3f %7.3f) (pre-adjust)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
aabb.max_.x(),
aabb.max_.y(),
aabb.max_.z());
aabb.min_.x() = std::floor(aabb.min_.x() / resolution) * resolution;
aabb.min_.y() = std::floor(aabb.min_.y() / resolution) * resolution;
aabb.min_.z() = std::floor(aabb.min_.z() / resolution) * resolution;
logInform(" DF: min=(%7.3f %7.3f %7.3f) max=(%7.3f %7.3f %7.3f) (post-adjust)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
aabb.max_.x(),
aabb.max_.y(),
aabb.max_.z());
Eigen::Vector3d size = aabb.max_ - aabb.min_;
double diagonal = size.norm();
logInform(" DF: sz=(%7.3f %7.3f %7.3f) cnt=(%d %d %d) diag=%f",
size.x(),
size.y(),
size.z(),
int(size.x()/resolution),
int(size.y()/resolution),
int(size.z()/resolution),
diagonal);
distance_field::PropagationDistanceField df(
size.x(),
size.y(),
size.z(),
resolution,
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
diagonal * 2.0,
true);
df.addPointsToField(points);
DistPosEntry default_entry;
default_entry.distance_ = diagonal * 2.0;
default_entry.cell_id_ = -1;
resize(size.x(),
size.y(),
size.z(),
resolution,
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
default_entry);
logInform(" copy %d points.",
getNumCells(distance_field::DIM_X) *
getNumCells(distance_field::DIM_Y) *
getNumCells(distance_field::DIM_Z));
int pdf_x,pdf_y,pdf_z;
int sdf_x,sdf_y,sdf_z;
Eigen::Vector3d pdf_p, sdf_p;
df.worldToGrid(aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), pdf_x,pdf_y,pdf_z);
worldToGrid(aabb.min_.x(), aabb.min_.y(), aabb.min_.z(), sdf_x,sdf_y,sdf_z);
df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z());
gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z());
logInform(" DF: min=(%10.6f %10.6f %10.6f) quant->%3d %3d %3d (pdf)",
aabb.min_.x(),
aabb.min_.y(),
aabb.min_.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: min=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (pdf)",
pdf_p.x(),
pdf_p.y(),
pdf_p.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: min=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (sdf)",
sdf_p.x(),
sdf_p.y(),
sdf_p.z(),
sdf_x,
sdf_y,
sdf_z);
df.worldToGrid(0,0,0, pdf_x,pdf_y,pdf_z);
worldToGrid(0,0,0, sdf_x,sdf_y,sdf_z);
df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z());
gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z());
logInform(" DF: org=(%10.6f %10.6f %10.6f) quant->%3d %3d %3d (pdf)",
0.0,
0.0,
0.0,
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: org=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (pdf)",
pdf_p.x(),
pdf_p.y(),
pdf_p.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: org=(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (sdf)",
sdf_p.x(),
sdf_p.y(),
sdf_p.z(),
sdf_x,
sdf_y,
sdf_z);
df.worldToGrid(points[0].x(), points[0].y(), points[0].z(), pdf_x,pdf_y,pdf_z);
worldToGrid(points[0].x(), points[0].y(), points[0].z(), sdf_x,sdf_y,sdf_z);
df.gridToWorld(pdf_x,pdf_y,pdf_z, pdf_p.x(), pdf_p.y(), pdf_p.z());
gridToWorld(sdf_x,sdf_y,sdf_z, sdf_p.x(), sdf_p.y(), sdf_p.z());
logInform(" DF: p0 =(%10.6f %10.6f %10.6f) quant->%3d %3d %3d (pdf)",
points[0].x(),
points[0].y(),
points[0].z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: p0 =(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (pdf)",
pdf_p.x(),
pdf_p.y(),
pdf_p.z(),
pdf_x,
pdf_y,
pdf_z);
logInform(" DF: p0 =(%10.6f %10.6f %10.6f) quant<-%3d %3d %3d (sdf)",
sdf_p.x(),
sdf_p.y(),
sdf_p.z(),
sdf_x,
sdf_y,
sdf_z);
for (int z = 0 ; z < df.getZNumCells() ; ++z)
{
for (int y = 0 ; y < df.getYNumCells() ; ++y)
{
for (int x = 0 ; x < df.getXNumCells() ; ++x)
{
DistPosEntry entry;
double dist = df.getDistance(x, y, z);
const distance_field::PropDistanceFieldVoxel& voxel = df.getCell(x,y,z);
if (dist < 0)
{
// propogation distance field has a bias of -1*resolution on points inside the object
if (dist <= -resolution)
{
dist += resolution;
}
else
{
logError("PropagationDistanceField returned distance=%f between 0 and -resolution=%f."
" Did someone fix it?"
" Need to remove workaround from static_distance_field.cpp",
dist,-resolution);
dist = 0.0;
}
entry.distance_ = dist;
entry.cell_id_ = getCellId(
voxel.closest_negative_point_.x(),
voxel.closest_negative_point_.y(),
voxel.closest_negative_point_.z());
}
else
{
entry.distance_ = dist;
entry.cell_id_ = getCellId(
voxel.closest_point_.x(),
voxel.closest_point_.y(),
voxel.closest_point_.z());
}
setCell(x, y, z, entry);
}
}
}
if (save_points)
std::swap(points, points_);
}
