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; }
void teleop_tracking::combineVertices(const std::vector<teleop_tracking::StlLoader::Facet> &facets, EigenSTL::vector_Vector3d &vertices, EigenSTL::vector_Vector3d &face_normals, std::vector<unsigned> &face_indices) { // The assumption is that these source are empty assert(vertices.empty()); assert(face_normals.empty()); assert(face_indices.empty()); EigenSTL::vector_Vector3f float_vector; for (std::size_t i = 0; i < facets.size(); ++i) { const StlLoader::Facet& f = facets[i]; face_normals.push_back(toEigend(f.normal).normalized()); unsigned v0 = appendUnique(float_vector, toEigenf(f.vertices[0])); unsigned v1 = appendUnique(float_vector, toEigenf(f.vertices[1])); unsigned v2 = appendUnique(float_vector, toEigenf(f.vertices[2])); // Small triangles should not have edges collapsed together assert(v0 != v1); assert(v0 != v2); assert(v1 != v2); face_indices.push_back(v0); face_indices.push_back(v1); face_indices.push_back(v2); } // copy the vector of single precision floats to double precision output for (std::size_t i = 0; i < float_vector.size(); ++i) { Eigen::Vector3d v = float_vector[i].cast<double>(); vertices.push_back(v); } }
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); }
Mesh* createMeshFromAsset(const aiScene* scene, const Eigen::Vector3d &scale, const std::string &resource_name) { if (!scene->HasMeshes()) { logWarn("Assimp reports scene in %s has no meshes", resource_name.c_str()); return NULL; } EigenSTL::vector_Vector3d vertices; std::vector<unsigned int> triangles; extractMeshData(scene, scene->mRootNode, aiMatrix4x4(), scale, vertices, triangles); if (vertices.empty()) { logWarn("There are no vertices in the scene %s", resource_name.c_str()); return NULL; } if (triangles.empty()) { logWarn("There are no triangles in the scene %s", resource_name.c_str()); return NULL; } return createMeshFromVertices(vertices, triangles); }
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_); }