static void extractMeshData(const aiScene *scene, const aiNode *node, const aiMatrix4x4 &parent_transform, const Eigen::Vector3d &scale,
                            EigenSTL::vector_Vector3d &vertices, std::vector<unsigned int> &triangles)
{
  aiMatrix4x4 transform = parent_transform;
  transform *= node->mTransformation;
  for (unsigned int j = 0 ; j < node->mNumMeshes; ++j)
  {
    const aiMesh* a = scene->mMeshes[node->mMeshes[j]];
    unsigned int offset = vertices.size();    
    for (unsigned int i = 0 ; i < a->mNumVertices ; ++i)
    {
      aiVector3D v = transform * a->mVertices[i];
      vertices.push_back(Eigen::Vector3d(v.x * scale.x(), v.y * scale.y(), v.z * scale.z()));
    }
    for (unsigned int i = 0 ; i < a->mNumFaces ; ++i)
      if (a->mFaces[i].mNumIndices == 3)
      {
        triangles.push_back(offset + a->mFaces[i].mIndices[0]);
        triangles.push_back(offset + a->mFaces[i].mIndices[1]);
        triangles.push_back(offset + a->mFaces[i].mIndices[2]);
      }
  }
  
  for (unsigned int n = 0; n < node->mNumChildren; ++n)
    extractMeshData(scene, node->mChildren[n], transform, scale, vertices, triangles);
}
示例#2
0
void mesh_core::Plane::leastSquaresGeneral(
      const EigenSTL::vector_Vector3d& points,
      Eigen::Vector3d* average)
{
  if (points.empty())
  {
    normal_ = Eigen::Vector3d(0,0,1);
    d_ = 0;
    if (average)
      *average = Eigen::Vector3d::Zero();
    return;
  }

  // find c, the average of the points
  Eigen::Vector3d c;
  c.setZero();

  EigenSTL::vector_Vector3d::const_iterator p = points.begin();
  EigenSTL::vector_Vector3d::const_iterator end = points.end();
  for ( ; p != end ; ++p)
    c += *p;

  c *= 1.0/double(points.size());

  // Find the matrix
  Eigen::Matrix3d m;
  m.setZero();

  p = points.begin();
  for ( ; p != end ; ++p)
  {
    Eigen::Vector3d cp = *p - c;
    m(0,0) += cp.x() * cp.x();
    m(1,0) += cp.x() * cp.y();
    m(2,0) += cp.x() * cp.z();
    m(1,1) += cp.y() * cp.y();
    m(2,1) += cp.y() * cp.z();
    m(2,2) += cp.z() * cp.z();
  }
  m(0,1) = m(1,0);
  m(0,2) = m(2,0);
  m(1,2) = m(2,1);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(m);
  if (eigensolver.info() == Eigen::Success)
  {
    normal_ = eigensolver.eigenvectors().col(0);
    normal_.normalize();
  }
  else
  {
    normal_ = Eigen::Vector3d(0,0,1);
  }

  d_ = -c.dot(normal_);

  if (average)
    *average = c;
}
示例#3
0
mesh_core::Plane::Plane(
      const EigenSTL::vector_Vector3d& points)
{
  if (points.size() <= 3)
    from3Points(points);
  else
    leastSquaresGeneral(points);
}
SphereInfo::SphereInfo(
      const EigenSTL::vector_Vector3d& points)
  : points_(points)
  , center_(0,0,0)
  , radius_(0)
  , radius_sq_(0)
{
  int npoints = points.size();
  list_.resize(npoints);
  for (int i = 0 ; i < npoints ; i++)
    list_[i] = &points[i];
}
void mesh_core::generateBoundingSphere(
      const EigenSTL::vector_Vector3d& points,
      Eigen::Vector3d& center,
      double &radius)
{
  SphereInfo info(points);

  info.findStartingPoints();
  info.findSphere(0, points.size());

  center = info.center_;
  radius = info.radius_;
}
示例#6
0
void mesh_core::appendPoints(
    EigenSTL::vector_Vector3d& vector,
    int npoints,
    const double *data)
{
  int base = vector.size();
  vector.resize(base + npoints);
  for (int i = 0; i < npoints ; ++i)
  {
    vector[base+i] = Eigen::Vector3d(data[i*3+0],
                                     data[i*3+1],
                                     data[i*3+2]);
  }
}
bool robot_sphere_representation::RobotSphereRepresentation::saveToSrdfFile(const std::string& srdf_filename) const
{
  genSpheresForAllLinks();

  // Get an SRDFWriter with the data from the current RobotModel
  moveit_setup_assistant::SRDFWriter writer;
  writer.initModel( *robot_model_->getURDF(), *robot_model_->getSRDF() );

  // Delete any existing spheres
  writer.link_sphere_approximations_.clear();

  // Insert generated spheres into SRDFWriter
  EigenSTL::vector_Vector3d centers;
  std::vector<double> radii;
  std::map<std::string, LinkSphereRepresentation*>::const_iterator lsr = links_.begin();
  std::map<std::string, LinkSphereRepresentation*>::const_iterator lsr_end = links_.end();
  for ( ; lsr != lsr_end ; ++lsr )
  {
    centers.clear();
    radii.clear();
    lsr->second->getSpheres(centers, radii);

    if (centers.empty())
    {
      // a link with no geometry is represented by a single radius=0 sphere
      radii.clear();
      radii.push_back(0);
      centers.push_back(Eigen::Vector3d(0,0,0));
    }

    srdf::Model::LinkSpheres lsp;
    lsp.link_ = lsr->first;

    for ( std::size_t i = 0 ; i < centers.size() ; ++i )
    {
      srdf::Model::Sphere sphere;
      sphere.center_x_ = centers[i].x();
      sphere.center_y_ = centers[i].y();
      sphere.center_z_ = centers[i].z();
      sphere.radius_ = radii[i];

      lsp.spheres_.push_back(sphere);
    }

    writer.link_sphere_approximations_.push_back(lsp);
  }

  // write the SRDF to file and return true on success.
  return writer.writeSRDF(srdf_filename);
}
TEST(SphereRayIntersection, SimpleRay2)
{
    shapes::Sphere shape(1.0);
    bodies::Body* sphere = new bodies::Sphere(&shape);
    sphere->setScale(1.05);

    Eigen::Vector3d ray_o(5, 0, 0);
    Eigen::Vector3d ray_d(1, 0, 0);
    EigenSTL::vector_Vector3d p;
    bool intersect = sphere->intersectsRay(ray_o, ray_d, &p);

    delete sphere;
    EXPECT_FALSE(intersect);
    EXPECT_EQ(0, (int)p.size());
}
TEST(SphereRayIntersection, SimpleRay1)
{
    shapes::Sphere shape(1.0);
    bodies::Body* sphere = new bodies::Sphere(&shape);
    sphere->setScale(1.05);

    Eigen::Vector3d ray_o(5, 0, 0);
    Eigen::Vector3d ray_d(-1, 0, 0);
    EigenSTL::vector_Vector3d p;
    bool intersect = sphere->intersectsRay(ray_o, ray_d, &p);

    delete sphere;
    EXPECT_TRUE(intersect);
    EXPECT_EQ(2, (int)p.size());
    EXPECT_NEAR(p[0].x(), 1.05, 1e-6);
    EXPECT_NEAR(p[1].x(), -1.05, 1e-6);
}
示例#10
0
void mesh_core::Plane::from3Points(
      const EigenSTL::vector_Vector3d& points)
{
  Eigen::Vector3d ab, ac, norm;
  int npoints = points.size();
  if (npoints > 2)
  {
    ab = points[1] - points[0];
    ac = points[2] - points[0];
    norm = ab.cross(ac);
  }
  else if (npoints == 2)
  {
    ab = points[1] - points[0];
    ac(0) = ab(1);
    ac(1) = ab(2);
    ac(2) = ab(0);
    norm = ab.cross(ac);
  }
  else if (npoints == 1)
  {
    *this = Plane(Eigen::Vector3d(0,0,1), points[0]);
    return;
  }
  else
  {
    *this = Plane();
    return;
  }

  if (norm.squaredNorm() <= std::numeric_limits<double>::epsilon())
  {
    ac(0) = ab(1);
    ac(1) = ab(2);
    ac(2) = ab(0);
    norm = ab.cross(ac);
    if (norm.squaredNorm() <= std::numeric_limits<double>::epsilon())
    {
      norm = Eigen::Vector3d(0,0,1);
    }
  }

  *this = Plane(norm, points[0]);
}
示例#11
0
void mesh_core::Plane::leastSquaresFast(
      const EigenSTL::vector_Vector3d& points,
      Eigen::Vector3d* average)
{
  Eigen::Matrix3d m;
  Eigen::Vector3d b;
  Eigen::Vector3d c;

  m.setZero();
  b.setZero();
  c.setZero();

  EigenSTL::vector_Vector3d::const_iterator p = points.begin();
  EigenSTL::vector_Vector3d::const_iterator end = points.end();
  for ( ; p != end ; ++p)
  {
    m(0,0) += p->x() * p->x();
    m(1,0) += p->x() * p->y();
    m(2,0) += p->x();
    m(1,1) += p->y() * p->y();
    m(2,1) += p->y();
    b(0) += p->x() * p->z();
    b(1) += p->y() * p->z();
    b(2) += p->z();
    c += *p;
  }
  m(0,1) = m(1,0);
  m(0,2) = m(2,0);
  m(1,2) = m(2,1);
  m(2,2) = double(points.size());
  c *= 1.0/double(points.size());

  normal_ = m.colPivHouseholderQr().solve(b);
  if (normal_.squaredNorm() > std::numeric_limits<double>::epsilon())
    normal_.normalize();

  d_ = -c.dot(normal_);

  if (average)
    *average = c;
}
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_);
}