コード例 #1
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
bool do_ConvexHull(meshproc_msgs::ConvexHull::Request &req,
                   meshproc_msgs::ConvexHull::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
    res.mesh_A_loaded = true;
    res.file_written = false;
    res.result = shape_msgs::Mesh();
    if(itA == loadedMeshes.end())
    {
        res.mesh_A_loaded = false;
        return true;
    }

    MeshMap::iterator itR = loadedMeshes.find(req.mesh_R);
    if(itR == loadedMeshes.end())
    {
        itR = loadedMeshes.insert(loadedMeshes.begin(),
                                  std::pair<std::string, MeshEntry*>(req.mesh_R, new MeshEntry()));
    }
    MeshEntry *A, *R;
    A = itA->second;
    R = itR->second;
    R->setFromConvexHull(*A);
    if(req.return_result)
        R->writeToMsg(res.result);
    else
        res.result = shape_msgs::Mesh();
    if(req.result_to_file)
    {
        res.file_written = R->writeToFile(req.result_filename);
    }
    return true;
}
コード例 #2
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
bool do_SolidifyMesh(meshproc_msgs::SolidifyMesh::Request &req,
                     meshproc_msgs::SolidifyMesh::Response &res)
{
    bool already_had_R = true;
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
    res.mesh_A_loaded = true;
    res.file_written = false;
    res.result = shape_msgs::Mesh();
    if(itA == loadedMeshes.end())
    {
        res.mesh_A_loaded = false;
        return true;
    }

    MeshMap::iterator itR = loadedMeshes.find(req.mesh_R);
    if(itR == loadedMeshes.end())
    {
        already_had_R = false;
        itR = loadedMeshes.insert(loadedMeshes.begin(),
                                  std::pair<std::string, MeshEntry*>(req.mesh_R, new MeshEntry()));
    }
    MeshEntry *A, *R;
    A = itA->second;
    R = itR->second;
    R->setFromSolidification(*A, req.thickness);
    if(req.return_result)
        R->writeToMsg(res.result);
    else
        res.result = shape_msgs::Mesh();
    if(req.result_to_file)
    {
        res.file_written = R->writeToFile(req.result_filename);
    }
    return true;
}
コード例 #3
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
bool do_ProjectMesh(meshproc_msgs::ProjectMesh::Request &req,
                    meshproc_msgs::ProjectMesh::Response &res)
{
    bool already_had_R = true;
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
    res.mesh_A_loaded = true;
    res.operation_performed = false;
    res.file_written = false;
    res.result = shape_msgs::Mesh();
    if(itA == loadedMeshes.end())
    {
        res.mesh_A_loaded = false;
        return true;
    }

    MeshMap::iterator itR = loadedMeshes.find(req.mesh_R);
    if(itR == loadedMeshes.end())
    {
        already_had_R = false;
        itR = loadedMeshes.insert(loadedMeshes.begin(),
                                  std::pair<std::string, MeshEntry*>(req.mesh_R, new MeshEntry()));
    }
    MeshEntry *A, *R;
    A = itA->second;
    R = itR->second;
    res.operation_performed = R->setFromProjection(*A, req.normal.x, req.normal.y, req.normal.z, req.fill_holes);
    if(!res.operation_performed)
    {
        if(!already_had_R)
        {
            delete R;
            loadedMeshes.erase(req.mesh_R);
        }
        return true;
    }
    if(req.return_result_as_mesh)
        R->writeToMsg(res.result);
    else
        res.result = shape_msgs::Mesh();
    if(req.return_result_as_polygon)
    {
        std::vector<double> x, y, z;
        R->getBoundaryPolygon(x, y, z, res.edge_L, res.edge_R);
        int maxK = x.size();
        for(int k = 0; k < maxK; k++)
        {
            geometry_msgs::Point aux;
            aux.x = x[k];
            aux.y = y[k];
            aux.z = z[k];
            res.points.push_back(aux);
        }
    }
    if(req.result_to_file)
    {
        res.file_written = R->writeToFile(req.result_filename);
    }
    return true;
}
コード例 #4
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
bool do_ConvexDecomposition(meshproc_msgs::ConvexDecomposition::Request &req,
                            meshproc_msgs::ConvexDecomposition::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
    res.mesh_loaded = true;
    res.nr_components = 0;
    res.mesh_names.clear();
    res.file_written.clear();
    res.components.clear();
    if(itA == loadedMeshes.end())
    {
        res.mesh_loaded = false;
        return true;
    }
    std::vector<MeshEntry*> components;
    components.clear();
    std::cerr << "Getting components" << std::endl;
    itA->second->getConvexComponents(components);
    std::cerr << "Components obtained" << std::endl;
    int maxK = res.nr_components = components.size();
    for(int k = 0; k < maxK; k++)
    {
        char nr_str[20];
        std::sprintf(nr_str, "%d", k);
        std::string cName = req.mesh_R + nr_str;
        MeshMap::iterator itR = loadedMeshes.find(cName);
        if(itR == loadedMeshes.end())
        {
            itR = loadedMeshes.insert(loadedMeshes.begin(),
                                      std::pair<std::string, MeshEntry*>(cName, components[k]));
        }
        else
        {
            delete itR->second;
            itR->second = components[k];
        }
        MeshEntry* R = itR->second;
        res.mesh_names.push_back(cName);
        if(req.return_result)
        {
            res.components.push_back(shape_msgs::Mesh());
            R->writeToMsg(res.components[k]);
        }
        if(req.result_to_file)
        {
            res.file_written.push_back(R->writeToFile(req.result_filename + nr_str + req.result_filename_extension));
            res.file_names.push_back(req.result_filename + nr_str + req.result_filename_extension);
        }

    }
    return true;
}
コード例 #5
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
bool do_GetMeshSkeleton(meshproc_msgs::GetMeshSkeleton::Request &req,
                        meshproc_msgs::GetMeshSkeleton::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_name);
    res.mesh_loaded = true;
    res.vertices.clear();
    res.edge_ends_L.clear();
    res.edge_ends_R.clear();
    if(itA == loadedMeshes.end())
    {
        res.mesh_loaded = false;
        return true;
    }
    std::vector<double> x, y, z;
    x.clear();
    y.clear();
    z.clear();
    itA->second->getMeshSkeleton(req.approximate, req.duplicate_distance, x, y, z, res.edge_ends_L, res.edge_ends_R);
    int maxK = x.size();
    res.vertices.reserve(maxK);
    for(int k = 0; k < maxK; k++)
    {
        geometry_msgs::Point aux;
        aux.x = x[k];
        aux.y = y[k];
        aux.z = z[k];
        res.vertices.push_back(aux);
    }
    return true;
}
コード例 #6
0
ファイル: scene_lua.cpp プロジェクト: SyedRagib/DesignProject
int gr_mesh_cmd(lua_State* L)
{
    GRLUA_DEBUG_CALL;

    gr_node_ud* data = (gr_node_ud*)lua_newuserdata(L, sizeof(gr_node_ud));
    data->node = 0;

    const char* name = luaL_checkstring(L, 1);
    const char* obj_fname = luaL_checkstring(L, 2);

	std::string sfname( obj_fname );

	// Use a dictionary structure to make sure every mesh is loaded
	// at most once.
	auto i = mesh_map.find( sfname );
	Mesh *mesh = nullptr;

	if( i == mesh_map.end() ) {
		mesh = new Mesh( obj_fname );
	} else {
		mesh = i->second;
	}

	data->node = new GeometryNode( name, mesh );

    luaL_getmetatable(L, "gr.node");
    lua_setmetatable(L, -2);

    return 1;
}
コード例 #7
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
bool do_GetNearMeshVertices(meshproc_msgs::GetNearMeshVertices::Request &req,
                            meshproc_msgs::GetNearMeshVertices::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_name);
    res.mesh_loaded = true;
    res.neighbors.clear();
    if(itA == loadedMeshes.end())
    {
        res.mesh_loaded = false;
        return true;
    }
    std::vector<MeshEntry::XYZTriplet> points;
    points.clear();
    itA->second->getNearVertices(req.point.x, req.point.y, req.point.z, req.distance, points);
    int maxK = points.size();
    for(int k = 0; k < maxK; k++)
    {
        geometry_msgs::Point aux;
        aux.x = points[k].x;
        aux.y = points[k].y;
        aux.z = points[k].z;
        res.neighbors.push_back(aux);
    }
    return true;
}
コード例 #8
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
bool do_UnloadMesh(meshproc_msgs::UnloadMesh::Request &req, meshproc_msgs::UnloadMesh::Response &res)
{
    res.unloaded_mesh = 0;
    MeshMap::iterator it = loadedMeshes.find(req.mesh_name);
    if(loadedMeshes.end() != it)
    {
        delete it->second;
        res.unloaded_mesh = 1;
        loadedMeshes.erase(it);
    }
    return true;
}
コード例 #9
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
MeshEntry* checkMeshAvailability(std::string const& meshName,
                                 meshproc_msgs::CSGRequest::Response::_mesh_A_loaded_type &isLoaded,
                                 meshproc_msgs::CSGRequest::Response::_mesh_A_csg_safe_type &isCSGSafe)
{
    isLoaded = isCSGSafe = false;
    MeshMap::iterator it = loadedMeshes.find(meshName);
    if(it != loadedMeshes.end())
    {
        isLoaded = true;
        isCSGSafe = it->second->isCSGSafe();
        return it->second;
    }
    return NULL;
}
コード例 #10
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
bool do_GetMeshAABB(meshproc_msgs::GetMeshAABB::Request &req,
                    meshproc_msgs::GetMeshAABB::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_name);
    res.mesh_loaded = true;
    if(itA == loadedMeshes.end())
    {
        res.mesh_loaded = false;
        return true;
    }
    double maxX, minX, maxY, minY, maxZ, minZ;
    itA->second->getBoundingBox(maxX, minX, maxY, minY, maxZ, minZ);
    res.max_x = maxX;
    res.min_x = minX;
    res.max_y = maxY;
    res.min_y = minY;
    res.max_z = maxZ;
    res.min_z = minZ;
    return true;
}
コード例 #11
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
bool do_PathOnMesh(meshproc_msgs::PathOnMesh::Request &req,
                   meshproc_msgs::PathOnMesh::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_name);
    res.mesh_loaded = true;
    res.operation_performed = false;

    if(itA == loadedMeshes.end())
    {
        res.mesh_loaded = false;
        return true;
    }

    MeshEntry *A;
    A = itA->second;

    res.operation_performed = A->getGeodesicPath(req.lina_start, req.lina_end, req.linb_start, req.linb_end,
                              res.path);

    return true;
}
コード例 #12
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
bool do_GetMesh(meshproc_msgs::GetMesh::Request &req,
                meshproc_msgs::GetMesh::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_name);
    res.mesh_loaded = true;
    res.file_written = false;
    if(itA == loadedMeshes.end())
    {
        res.mesh_loaded = false;
        return true;
    }
    if(req.return_result)
        itA->second->writeToMsg(res.result);
    else
        res.result = shape_msgs::Mesh();
    if(req.result_to_file)
    {
        res.file_written = itA->second->writeToFile(req.result_filename);
    }
    return true;
}
コード例 #13
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
bool do_LoadMesh(meshproc_msgs::LoadMesh::Request &req, meshproc_msgs::LoadMesh::Response &res)
{
    ROS_INFO("Loading mesh %s", req.mesh_name.c_str());
    res.loaded_mesh = res.io_error = res.mesh_already_loaded = false;
    MeshMap::iterator it = loadedMeshes.find(req.mesh_name);
    if(it != loadedMeshes.end())
        res.mesh_already_loaded = true;
    else
    {
        if((0 == req.mesh_filenames.size()) && (0 == req.mesh_msgs.size()))
            return true;
        it = loadedMeshes.insert(loadedMeshes.begin(),
                                 std::pair<std::string, MeshEntry*>(req.mesh_name, new MeshEntry()));
        MeshEntry *R = it->second;
        int maxK = req.mesh_filenames.size();
        bool goOn = true;
        for(int k = 0; (k < maxK) && goOn; k++)
        {
            ROS_INFO("    loading part from file %s", req.mesh_filenames[k].c_str());
            goOn = R->loadFromFile(req.mesh_filenames[k], req.duplicate_dist);
        }
        if(!goOn)
        {
            ROS_INFO("    Encountered I/O error (file might not exist or is inaccessible), cancelling load.");
            res.io_error = true;
            delete R;
            loadedMeshes.erase(it);
            return true;
        }
        res.loaded_mesh = true;
        maxK = req.mesh_msgs.size();
        for(int k = 0; k < maxK; k++)
        {
            ROS_INFO("    loading part from message");
            R->loadFromMsg(req.mesh_msgs[k], req.duplicate_dist);
        }
    }
    ROS_INFO("    Loading done.");
    return true;
}
コード例 #14
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
bool do_AffineTransformMesh(meshproc_msgs::AffineTransformMesh::Request &req,
                            meshproc_msgs::AffineTransformMesh::Response &res)
{
    MeshEntry *A, *R;
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
    MeshMap::iterator itR = loadedMeshes.find(req.mesh_R);
    res.mesh_A_loaded = true;
    res.file_written = false;
    if(itA == loadedMeshes.end())
    {
        res.mesh_A_loaded = false;
        return true;
    }
    if(itR == loadedMeshes.end())
    {
        itR = loadedMeshes.insert(loadedMeshes.begin(),
                                  std::pair<std::string, MeshEntry*>(req.mesh_R, new MeshEntry()));
    }
    A = itA->second;
    R = itR->second;
    R->setFromMeshEntry(*A);
    Eigen::Affine3d M = Eigen::Translation3d(req.transform.translation.x,
                        req.transform.translation.y,
                        req.transform.translation.z)*
                        Eigen::Quaterniond(req.transform.rotation.w,
                                           req.transform.rotation.x,
                                           req.transform.rotation.y,
                                           req.transform.rotation.z);
    R->applyTransform(M);
    if(req.return_result)
        R->writeToMsg(res.result);
    else
        res.result = shape_msgs::Mesh();
    if(req.result_to_file)
    {
        res.file_written = R->writeToFile(req.result_filename);
    }
    return true;
}
コード例 #15
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
bool do_GetMeshProps(meshproc_msgs::GetMeshProps::Request &req, meshproc_msgs::GetMeshProps::Response &res)
{
    MeshMap::iterator it = loadedMeshes.find(req.mesh_name);
    if(it != loadedMeshes.end())
    {
        res.is_loaded = true;
        res.is_closed = it->second->isClosed();
        res.is_manifold = it->second->isManifold();
        res.is_orientable = it->second->isOrientable();
        res.is_safe_for_csg = it->second->isCSGSafe();
        res.num_connected_components = it->second->getNrConnectedComponents();
        res.num_vertices = it->second->getNrVertices();
        res.num_edges = it->second->getNrEdges();
        res.num_faces = it->second->getNrFaces();
        res.Euler_characteristic = it->second->getEulerCharacteristic();
        res.volume = it->second->getVolume();
    }
    else
    {
        res.is_loaded = false;
    }
    return true;
}
コード例 #16
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
bool do_SelectByNormal(meshproc_msgs::SelectByNormal::Request &req,
                       meshproc_msgs::SelectByNormal::Response &res)
{
    MeshMap::iterator itA = loadedMeshes.find(req.mesh_A);
    res.mesh_A_loaded = true;
    res.file_written = false;
    res.operation_performed = false;
    res.result = shape_msgs::Mesh();
    if(itA == loadedMeshes.end())
    {
        res.mesh_A_loaded = false;
        return true;
    }

    MeshEntry *A;
    A = itA->second;

    res.operation_performed = A->filterByNormal(req.result_filename,
                              req.normal.x, req.normal.y, req.normal.z,
                              req.tolerated_angle);

    res.file_written = res.operation_performed;
    return true;
}
コード例 #17
0
cv::Mat projectWithEigen()
{
	// Transform meshes into camera frame

	// For each frame in vector
	for (int frame = 0; frame < mMeshFrameIDs.size(); frame++)
	{
		// Lookup current transform
		Eigen::Isometry3 transform;
		transform = transforms.at(mMeshFrameIDs[frame]);

		// Get copy of mesh for each frame
		ap::Mesh* sourceMesh;
		ap::Mesh* transformedMesh;
		//std::cerr << "Getting frame " << frame << " : " << mMeshFrameIDs[frame] << std::endl;
		MeshMap::iterator scene_i = scenes.find(mMeshFrameIDs[frame]);
		if (scenes.end() == scene_i) { continue; }
		sourceMesh = scene_i->second;

		MeshMap::iterator scene_t = transformedScenes.find(mMeshFrameIDs[frame]);
		if (transformedScenes.end() == scene_t) { continue; }
		transformedMesh = scene_t->second;

		// Transform mesh into camera frame
		for (int i = 0; i < sourceMesh->vertices.size(); i++)
		{
			Eigen::Vector3 newVertex = transform * sourceMesh->vertices[i];
			//std::cerr << mesh->vertices[i].transpose() << "\t->\t" << newVertex.transpose() << std::endl;
			transformedMesh->vertices[i] = newVertex;
		}
	}

	// For each pixel in camera image
	cv::Mat robotImage(mCameraModel.cameraInfo().height, mCameraModel.cameraInfo().width, CV_32F);
	float* pixelPtr = (float*)robotImage.data;
	float maxDepth = 0;
	for (int v = 0; v < robotImage.rows; v++)
	{
		for (int u = 0; u < robotImage.cols; u++)
		{
			// Create a ray through the pixel
			int pixelIdx = u + (v * robotImage.cols);
			//std::cerr << "Pixel (" << u << "," << v << ")" << std::endl;
			cv::Point2d pixel = cv::Point2d(u, v);
			cv::Point3d cvRay = mCameraModel.projectPixelTo3dRay(pixel);
			// Convert cvRay to ap::Ray
			ap::Ray ray;
			ray.point = Eigen::Vector3::Zero();
			ray.vector.x() = cvRay.x; ray.vector.y() = cvRay.y; ray.vector.z() = cvRay.z;
			ray.vector.normalize();
			//std::cerr << ray.vector.transpose() << std::endl;

			// For each frame in vector
			for (int frame = 0; frame < mMeshFrameIDs.size(); frame++)
			{
				MeshMap::iterator scene_i = transformedScenes.find(mMeshFrameIDs[frame]);
				if (transformedScenes.end() == scene_i)
				{
					continue;
				}
				ap::Mesh* mesh = scene_i->second;

				// For each triangle in mesh
				for (int i = 0; i < mesh->faces.size(); i++)
				{
					// Check for intersection. If finite, set distance

					ap::Triangle triangle(mesh->vertices[mesh->faces[i].vertices[0]],
										  mesh->vertices[mesh->faces[i].vertices[1]],
										  mesh->vertices[mesh->faces[i].vertices[2]]);

					Eigen::Vector3 intersection = ap::intersectRayTriangle(ray, triangle);
					if (std::isfinite(intersection.x()))
					{
						float d = intersection.norm();
						float val = pixelPtr[pixelIdx];
						if (val == 0 || val > d)
						{
							pixelPtr[pixelIdx] = d;
						}
						if (d > maxDepth)
						{
							maxDepth = d;
						}
					}
				}
			}
		}
	}

	// Return the matrix
	if (maxDepth == 0) { maxDepth = 1;}
	return robotImage/maxDepth;
}
コード例 #18
0
ファイル: csg.cpp プロジェクト: mpomarlan/meshproc
bool do_CSGRequest(meshproc_msgs::CSGRequest::Request &req, meshproc_msgs::CSGRequest::Response &res)
{
    res.operation_performed = false;
    res.mesh_A_csg_safe = res.mesh_A_loaded = res.mesh_B_csg_safe = res.mesh_B_loaded = false;
    res.result = shape_msgs::Mesh();
    bool already_had_R = true;
    MeshEntry *A, *B, *R;
    A = checkMeshAvailability(req.mesh_A, res.mesh_A_loaded, res.mesh_A_csg_safe);
    B = checkMeshAvailability(req.mesh_B, res.mesh_B_loaded, res.mesh_B_csg_safe);
    MeshMap::iterator it = loadedMeshes.find(req.mesh_R);
    if(it == loadedMeshes.end())
    {
        already_had_R = false;
        it = loadedMeshes.insert(loadedMeshes.begin(),
                                 std::pair<std::string, MeshEntry*>(req.mesh_R, new MeshEntry()));
    }
    R = it->second;
    if(!canPerform(res.mesh_A_loaded, res.mesh_B_loaded, res.mesh_A_csg_safe, res.mesh_B_csg_safe, req.operation))
    {
        //Operation can't be performed, so let's not leave a result mesh (if created just now) loaded as a side-effect.
        if(!already_had_R)
        {
            delete R;
            loadedMeshes.erase(req.mesh_R);
        }
        return true;
    }
    switch(req.operation)
    {
    case 0:
        res.operation_performed = R->setFromUnion(*A, *B);
        break;
    case 1:
        res.operation_performed = R->setFromIntersection(*A, *B);
        break;
    case 2:
        res.operation_performed = R->setFromDifference(*A, *B);
        break;
    case 3:
        res.operation_performed = R->setFromSymmetricDifference(*A, *B);
        break;
    case 4:
        res.operation_performed = R->setFromMinkowskiSum(*A, *B);
        break;
    case 5:
        res.operation_performed = R->setFromMinkowskiErosion(*A, *B);
        break;
    }

    if(!res.operation_performed)
    {
        //Operation failed, so let's not leave a result mesh (if created just now) loaded as a side-effect.
        if(!already_had_R)
        {
            delete R;
            loadedMeshes.erase(req.mesh_R);
        }
        return true;
    }

    if(req.return_result)
        R->writeToMsg(res.result);
    else
        res.result = shape_msgs::Mesh();
    if(req.result_to_file)
    {
        res.file_written = R->writeToFile(req.result_filename);
    }
    return true;
}