コード例 #1
0
ファイル: gridUtility.cpp プロジェクト: manteapi/utility
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;
}
コード例 #2
0
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());
}
コード例 #3
0
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);
        }
      }
    }
  }
}
コード例 #4
0
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));
}
コード例 #5
0
ファイル: gridUtility.cpp プロジェクト: manteapi/utility
SReal Grid3dUtility::length(int cellId1, int cellId2) const
{
    Vec3f wCoord1 = gridToWorld(cellId1);
    Vec3f wCoord2 = gridToWorld(cellId2);
    return (wCoord1-wCoord2).length();
}
コード例 #6
0
ファイル: gridUtility.cpp プロジェクト: manteapi/utility
Vec3f Grid3dUtility::gridToWorld(const int i) const
{
    Vec3i gCoord = gridCoord(i);
    Vec3f wCoord = gridToWorld(gCoord);
    return wCoord;
}
コード例 #7
0
ファイル: gridUtility.cpp プロジェクト: manteapi/utility
Vec2r Grid2dUtility::gridToWorld(const int i) const
{
    Vec2i gCoord = gridCoord(i);
    Vec2r wCoord = gridToWorld(gCoord);
    return wCoord;
}
コード例 #8
0
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_);
}