From 8cc154f2e017886ed47ad813799c695119f7911d Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski <112629916+msz-rai@users.noreply.github.com> Date: Thu, 22 Jun 2023 00:43:50 +0100 Subject: [PATCH] Implement material information reading (#156) Implement material information reading (#149) * Add API call to create mesh with uvs * Add texture placeholder * Cleanup * Add texture api call * Add Texture constructor with cuda objects * Add texture to SBT * Rename object to be consistent * Add texture to api objects * Add texture sampling to OptiX program * Change API Calls policy * Add placeholder for basic texture test * rename texture slot * Add texture coordinates to mesh * First porsion of small cleanups * change approach to TextureData struct * Add TextureData as a texture field * add channel descriptor creator * Fix potential issues with non square and tiling textures * Fix float interplotation bug * Fix texture sampling and write test to that * Define non hit value (#146) * Add texture sampling to OptiX program * Cleanup before PR * Make SBT request more elegant * Make TAPE_HOOK for void* array work * PR fix chabges v1 * Rebuild texture test and fix tape * Change texture logic to only float approach * Smol fixes * invalid param tests * change suit name * Update src/api/apiCore.cpp * Update test/src/textureTest.cpp * Update src/api/apiCore.cpp * Update src/Tape.hpp * Update src/api/apiCore.cpp * Add delete_texture call * PR changes v2 * PR changes v3 * Zero pointer and change date * change texture to char and small fixes * tweak optix to half-float textures * Playground commit * fix texture to 8bit char * Update test/src/tapeSurfaceTest.cpp * Fix PR changes * RGL_TEXTURE_TEXEL_FORMAT -> TextureTexelFormat * remove unsafe include in cuda --------- Co-authored-by: Piotr Mrozik <33581747+PiotrMrozik@users.noreply.github.com> --- CMakeLists.txt | 1 + include/rgl/api/core.h | 74 +++++++++++++-- src/RGLFields.hpp | 2 + src/Tape.cpp | 4 + src/Tape.hpp | 12 +++ src/api/apiCore.cpp | 94 ++++++++++++++++++ src/gpu/ShaderBindingTableTypes.h | 11 ++- src/gpu/optixPrograms.cu | 34 +++++-- src/repr.hpp | 12 +++ src/scene/Entity.cpp | 8 ++ src/scene/Entity.hpp | 6 ++ src/scene/Mesh.cpp | 68 ++++++++----- src/scene/Mesh.hpp | 11 ++- src/scene/Scene.cpp | 4 + src/scene/Texture.cpp | 87 +++++++++++++++++ src/scene/Texture.hpp | 51 ++++++++++ test/CMakeLists.txt | 1 + test/include/models.hpp | 13 ++- test/include/utils.hpp | 46 ++++++++- test/src/scene/textureTest.cpp | 152 ++++++++++++++++++++++++++++++ test/src/tapeSurfaceTest.cpp | 10 ++ 21 files changed, 651 insertions(+), 50 deletions(-) create mode 100644 src/scene/Texture.cpp create mode 100644 src/scene/Texture.hpp create mode 100644 test/src/scene/textureTest.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 2ec2bccd..7f0173b9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -82,6 +82,7 @@ add_library(RobotecGPULidar SHARED src/scene/Scene.cpp src/scene/Mesh.cpp src/scene/Entity.cpp + src/scene/Texture.cpp src/scene/ASBuildScratchpad.cpp src/graph/Graph.cpp src/graph/Node.cpp diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h index c13c97b1..a4b29d59 100644 --- a/include/rgl/api/core.h +++ b/include/rgl/api/core.h @@ -53,6 +53,14 @@ // It is assigned by default if the user does not specify it. #define RGL_DEFAULT_ENTITY_ID 2147483647 +/** + * Two consecutive 32-bit floats. + */ +typedef struct +{ + float value[2]; +} rgl_vec2f; + /** * Three consecutive 32-bit floats. */ @@ -86,24 +94,30 @@ typedef struct * Represents on-GPU mesh that can be referenced by entities on the scene. * Each mesh can be referenced by any number of entities on different scenes. */ -typedef struct Mesh *rgl_mesh_t; +typedef struct Mesh* rgl_mesh_t; /** * Opaque handle representing an object visible to lidars. * An entity is always bound to exactly one scene. */ -typedef struct Entity *rgl_entity_t; +typedef struct Entity* rgl_entity_t; + +/** + * Represents on-GPU texture that can be referenced by Entities on the scene. + * Each texture can be referenced by any number of Entities on different scenes. + */ +typedef struct Texture* rgl_texture_t; /** * TODO(prybicki) */ -typedef struct Node *rgl_node_t; +typedef struct Node* rgl_node_t; /** * Opaque handle representing a scene - a collection of entities. * Using scene is optional. NULL can be passed to use an implicit default scene. */ -typedef struct Scene *rgl_scene_t; +typedef struct Scene* rgl_scene_t; /** * Enumerates available extensions in RGL which can be queried using `rgl_get_extension_info`. @@ -313,12 +327,24 @@ rgl_cleanup(void); * @param index_count Number of elements in the indices array */ RGL_API rgl_status_t -rgl_mesh_create(rgl_mesh_t *out_mesh, - const rgl_vec3f *vertices, +rgl_mesh_create(rgl_mesh_t* out_mesh, + const rgl_vec3f* vertices, int32_t vertex_count, - const rgl_vec3i *indices, + const rgl_vec3i* indices, int32_t index_count); +/** + * Assign texture coordinates to given mesh. Pair of texture coordinates is assigned to each vertex. + * + * @param mesh Address to store the resulting mesh handle + * @param uvs An array of rgl_vec2f or binary-compatible data representing mesh uv coordinates + * @param vertex_count Number of elements in the vertices array. It has to be equal to vertex buffer size. + */ +RGL_API rgl_status_t +rgl_mesh_set_texture_coords(rgl_mesh_t mesh, + const rgl_vec2f* uvs, + int32_t uv_count); + /** * Informs that the given mesh will be no longer used. * The mesh will be destroyed after all referring entities are destroyed. @@ -336,10 +362,9 @@ rgl_mesh_destroy(rgl_mesh_t mesh); */ RGL_API rgl_status_t rgl_mesh_update_vertices(rgl_mesh_t mesh, - const rgl_vec3f *vertices, + const rgl_vec3f* vertices, int32_t vertex_count); - /******************************** ENTITY ********************************/ /** @@ -350,7 +375,7 @@ rgl_mesh_update_vertices(rgl_mesh_t mesh, * @param mesh Handle to the mesh which will represent the entity on the scene. */ RGL_API rgl_status_t -rgl_entity_create(rgl_entity_t *out_entity, rgl_scene_t scene, rgl_mesh_t mesh); +rgl_entity_create(rgl_entity_t* out_entity, rgl_scene_t scene, rgl_mesh_t mesh); /** * Removes an entity from the scene and releases its resources (memory). @@ -376,6 +401,35 @@ rgl_entity_set_pose(rgl_entity_t entity, const rgl_mat3x4f *transform); RGL_API rgl_status_t rgl_entity_set_id(rgl_entity_t entity, int32_t id); +/** + * Assign intensity texture to the given entity. The assumption is that the entity can hold only one intensity texture. + * @param entity Entity to modify. + * @apram texture Texture to assign. + */ +RGL_API rgl_status_t +rgl_entity_set_intensity_texture(rgl_entity_t entity, rgl_texture_t texture); + +/******************************* TEXTURE *******************************/ + +/** + * Creates a Texture. + * Texture is a container object which holds device pointer to texture resource. + * @param out_texture Handle to the created Texture. + * @param texels Pointer to the texture data. Should be pass as raw byte data of unsigned char array . + * @param width Width of the texture. Has to be positive. + * @param height Height of the texture. It is not demanded that width == height. Has to be positive. + */ +RGL_API rgl_status_t +rgl_texture_create(rgl_texture_t* out_texture, const void* texels, int32_t width, int32_t height); + +/** + * Informs that the given texture will be no longer used. + * The texture will be destroyed after all referring entities are destroyed. + * @param mesh Texture to be marked as no longer needed + */ +RGL_API rgl_status_t +rgl_texture_destroy(rgl_texture_t texture); + /******************************** SCENE ********************************/ /** diff --git a/src/RGLFields.hpp b/src/RGLFields.hpp index 5f809533..2a43252a 100644 --- a/src/RGLFields.hpp +++ b/src/RGLFields.hpp @@ -27,6 +27,8 @@ #define NON_HIT_VALUE FLT_MAX +typedef unsigned char TextureTexelFormat; + // Shorter versions to avoid long type names #define XYZ_F32 RGL_FIELD_XYZ_F32 #define IS_HIT_I32 RGL_FIELD_IS_HIT_I32 diff --git a/src/Tape.cpp b/src/Tape.cpp index b5454ed6..e0f702f1 100644 --- a/src/Tape.cpp +++ b/src/Tape.cpp @@ -71,9 +71,13 @@ TapePlayer::TapePlayer(const char* path) { "rgl_mesh_create", std::bind(&TapePlayer::tape_mesh_create, this, _1) }, { "rgl_mesh_destroy", std::bind(&TapePlayer::tape_mesh_destroy, this, _1) }, { "rgl_mesh_update_vertices", std::bind(&TapePlayer::tape_mesh_update_vertices, this, _1) }, + { "rgl_mesh_set_texture_coords", std::bind(&TapePlayer::tape_mesh_set_texture_coords, this, _1) }, + { "rgl_texture_create", std::bind(&TapePlayer::tape_texture_create, this, _1) }, + { "rgl_texture_destroy", std::bind(&TapePlayer::tape_texture_destroy, this, _1) }, { "rgl_entity_create", std::bind(&TapePlayer::tape_entity_create, this, _1) }, { "rgl_entity_destroy", std::bind(&TapePlayer::tape_entity_destroy, this, _1) }, { "rgl_entity_set_pose", std::bind(&TapePlayer::tape_entity_set_pose, this, _1) }, + { "rgl_entity_set_intensity_texture", std::bind(&TapePlayer::tape_entity_set_intensity_texture, this, _1) }, { "rgl_scene_set_time", std::bind(&TapePlayer::tape_scene_set_time, this, _1) }, { "rgl_graph_run", std::bind(&TapePlayer::tape_graph_run, this, _1) }, { "rgl_graph_destroy", std::bind(&TapePlayer::tape_graph_destroy, this, _1) }, diff --git a/src/Tape.hpp b/src/Tape.hpp index c1109d9c..3a9c45f3 100644 --- a/src/Tape.hpp +++ b/src/Tape.hpp @@ -120,6 +120,9 @@ class TapeRecorder uintptr_t valueToYaml(rgl_mesh_t value) { return (uintptr_t) value; } uintptr_t valueToYaml(rgl_mesh_t* value) { return (uintptr_t) *value; } + uintptr_t valueToYaml(rgl_texture_t value) { return (uintptr_t) value; } + uintptr_t valueToYaml(rgl_texture_t* value) { return (uintptr_t) *value; } + uintptr_t valueToYaml(rgl_scene_t value) { return (uintptr_t) value; } uintptr_t valueToYaml(rgl_scene_t* value) { return (uintptr_t) *value; } @@ -140,6 +143,9 @@ class TapeRecorder template size_t valueToYaml(std::pair value) { return writeToBin(value.first, value.second); } + template + size_t valueToYaml(std::pair value) { return writeToBin(static_cast(value.first), value.second); } + public: explicit TapeRecorder(const std::filesystem::path& path); @@ -180,6 +186,7 @@ struct TapePlayer std::unordered_map tapeMeshes; std::unordered_map tapeEntities; + std::unordered_map tapeTextures; std::unordered_map tapeNodes; std::map> tapeFunctions; @@ -195,10 +202,14 @@ struct TapePlayer void tape_mesh_create(const YAML::Node& yamlNode); void tape_mesh_destroy(const YAML::Node& yamlNode); void tape_mesh_update_vertices(const YAML::Node& yamlNode); + void tape_mesh_set_texture_coords(const YAML::Node& yamlNode); + void tape_texture_create(const YAML::Node &yamlNode); + void tape_texture_destroy(const YAML::Node &yamlNode); void tape_entity_create(const YAML::Node& yamlNode); void tape_entity_destroy(const YAML::Node& yamlNode); void tape_entity_set_pose(const YAML::Node& yamlNode); void tape_entity_set_id(const YAML::Node& yamlNode); + void tape_entity_set_intensity_texture(const YAML::Node &yamlNode); void tape_scene_set_time(const YAML::Node& yamlNode); void tape_graph_run(const YAML::Node& yamlNode); void tape_graph_destroy(const YAML::Node& yamlNode); @@ -231,6 +242,7 @@ struct TapePlayer void tape_node_points_ros2_publish(const YAML::Node& yamlNode); void tape_node_points_ros2_publish_with_qos(const YAML::Node& yamlNode); #endif + }; extern std::optional tapeRecorder; diff --git a/src/api/apiCore.cpp b/src/api/apiCore.cpp index 597b27ea..d05881f8 100644 --- a/src/api/apiCore.cpp +++ b/src/api/apiCore.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -141,6 +142,7 @@ rgl_cleanup(void) CHECK_CUDA(cudaStreamSynchronize(nullptr)); Entity::instances.clear(); Mesh::instances.clear(); + Texture::instances.clear(); Scene::defaultInstance()->clear(); while (!Node::instances.empty()) { // Note: Graph::destroy calls Node::release() to remove its from APIObject::instances @@ -191,6 +193,30 @@ void TapePlayer::tape_mesh_create(const YAML::Node& yamlNode) tapeMeshes.insert(std::make_pair(yamlNode[0].as(), mesh)); } +RGL_API rgl_status_t +rgl_mesh_set_texture_coords(rgl_mesh_t mesh, const rgl_vec2f* uvs, int32_t uv_count) { + auto status = rglSafeCall([&]() { + RGL_API_LOG("rgl_mesh_set_texture_coords(mesh={}, uvs={}, uv_count={})", + (void*) mesh, repr(uvs, uv_count), uv_count); + CHECK_ARG(mesh != nullptr); + CHECK_ARG(uvs != nullptr); + CHECK_CUDA(cudaStreamSynchronize(nullptr)); + CHECK_ARG(uv_count == Mesh::validatePtr(mesh)->getVertexCount()); + + Mesh::validatePtr(mesh)->setTexCoords(reinterpret_cast(uvs), uv_count); + + }); + TAPE_HOOK(mesh, TAPE_ARRAY(uvs, uv_count), uv_count); + return status; +} + +void TapePlayer::tape_mesh_set_texture_coords(const YAML::Node& yamlNode) +{ + rgl_mesh_set_texture_coords(tapeMeshes.at(yamlNode[0].as()), + reinterpret_cast(fileMmap + yamlNode[1].as()), + yamlNode[2].as()); +} + RGL_API rgl_status_t rgl_mesh_destroy(rgl_mesh_t mesh) { @@ -323,6 +349,74 @@ void TapePlayer::tape_entity_set_id(const YAML::Node& yamlNode) yamlNode[1].as::type>()); } +RGL_API rgl_status_t +rgl_entity_set_intensity_texture(rgl_entity_t entity, rgl_texture_t texture ) +{ + auto status = rglSafeCall([&](){ + RGL_API_LOG("rgl_entity_set_intensity_texture(entity={}, texture={})", (void*) entity, (void*) texture); + CHECK_ARG(entity != nullptr); + CHECK_ARG(texture != nullptr); + Entity::validatePtr(entity)->setIntensityTexture(Texture::validatePtr(texture)); + }); + + TAPE_HOOK(entity, texture); + return status; +} + +void TapePlayer::tape_entity_set_intensity_texture(const YAML::Node &yamlNode) +{ + rgl_entity_set_intensity_texture(tapeEntities.at(yamlNode[0].as()), + tapeTextures.at(yamlNode[1].as())); +} + +RGL_API rgl_status_t +rgl_texture_create(rgl_texture_t* out_texture, const void* texels, int32_t width, int32_t height) +{ + auto status = rglSafeCall([&]() { + RGL_API_LOG("rgl_texture_create(out_texture={}, width={}, height={})", (void*) out_texture, width, height); + CHECK_ARG(out_texture != nullptr); + CHECK_ARG(texels != nullptr); + CHECK_ARG(width > 0); + CHECK_ARG(height > 0); + + *out_texture = Texture::create(texels, width, height).get(); + }); + TAPE_HOOK(out_texture, TAPE_ARRAY(texels, (width * height * sizeof(TextureTexelFormat))), width, height); + return status; +} + +void TapePlayer::tape_texture_create(const YAML::Node& yamlNode) +{ + rgl_texture_t texture = nullptr; + + rgl_texture_create(&texture, + reinterpret_cast(fileMmap + yamlNode[1].as()), + yamlNode[2].as(), + yamlNode[3].as()); + + tapeTextures.insert(std::make_pair(yamlNode[0].as(), texture)); +} + +RGL_API rgl_status_t +rgl_texture_destroy(rgl_texture_t texture) +{ + auto status = rglSafeCall([&]() { + RGL_API_LOG("rgl_texture_destroy(texture={})", (void*) texture); + CHECK_ARG(texture != nullptr); + CHECK_CUDA(cudaStreamSynchronize(nullptr)); + Texture::release(texture); + }); + TAPE_HOOK(texture); + return status; +} + +void TapePlayer::tape_texture_destroy(const YAML::Node &yamlNode) +{ + auto textureId = yamlNode[0].as(); + rgl_texture_destroy(tapeTextures.at(textureId)); + tapeTextures.erase(textureId); +} + RGL_API rgl_status_t rgl_scene_set_time(rgl_scene_t scene, uint64_t nanoseconds) { diff --git a/src/gpu/ShaderBindingTableTypes.h b/src/gpu/ShaderBindingTableTypes.h index 8e6bbd64..b8f42ea9 100644 --- a/src/gpu/ShaderBindingTableTypes.h +++ b/src/gpu/ShaderBindingTableTypes.h @@ -4,11 +4,16 @@ #include struct TriangleMeshSBTData { - const Vec3f *vertex; - const Vec3i *index; + const Vec3f* vertex; + const Vec3i* index; size_t vertex_count; size_t index_count; - int entity_id; + + int entity_id; + + const Vec2f* texture_coords; + size_t texture_coords_count; + cudaTextureObject_t texture; }; diff --git a/src/gpu/optixPrograms.cu b/src/gpu/optixPrograms.cu index 9604d2b8..c3a133da 100644 --- a/src/gpu/optixPrograms.cu +++ b/src/gpu/optixPrograms.cu @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -56,7 +57,7 @@ Vec3f decodePayloadVec3f(const Vec3fPayload& src) template __forceinline__ __device__ -void saveRayResult(const Vec3f* xyz=nullptr, const Vec3f* origin=nullptr, const int objectID = RGL_ENTITY_INVALID_ID) +void saveRayResult(const Vec3f* xyz=nullptr, const Vec3f* origin=nullptr, float intensity=0, const int objectID=RGL_ENTITY_INVALID_ID) { const int rayIdx = optixGetLaunchIndex().x; if (ctx.xyz != nullptr) { @@ -81,14 +82,14 @@ void saveRayResult(const Vec3f* xyz=nullptr, const Vec3f* origin=nullptr, const : NON_HIT_VALUE; } if (ctx.intensity != nullptr) { - ctx.intensity[rayIdx] = 100; + ctx.intensity[rayIdx] = intensity; } if (ctx.timestamp != nullptr) { ctx.timestamp[rayIdx] = ctx.sceneTime; } - if (ctx.entityId != nullptr) { - ctx.entityId[rayIdx] = isFinite ? objectID : RGL_ENTITY_INVALID_ID; - } + if (ctx.entityId != nullptr) { + ctx.entityId[rayIdx] = isFinite ? objectID : RGL_ENTITY_INVALID_ID; + } } extern "C" __global__ void __raygen__() @@ -122,6 +123,7 @@ extern "C" __global__ void __closesthit__() assert(index.x() < sbtData.vertex_count); assert(index.y() < sbtData.vertex_count); assert(index.z() < sbtData.vertex_count); + const Vec3f& A = sbtData.vertex[index.x()]; const Vec3f& B = sbtData.vertex[index.y()]; const Vec3f& C = sbtData.vertex[index.z()]; @@ -129,14 +131,32 @@ extern "C" __global__ void __closesthit__() Vec3f hitObject = Vec3f((1 - u - v) * A + u * B + v * C); Vec3f hitWorld = optixTransformPointFromObjectToWorldSpace(hitObject); - int objectID = sbtData.entity_id; + int objectID = sbtData.entity_id; Vec3f origin = decodePayloadVec3f({ optixGetPayload_0(), optixGetPayload_1(), optixGetPayload_2() }); - saveRayResult(&hitWorld, &origin, objectID); + + float intensity = 0; + if (sbtData.texture_coords != nullptr && sbtData.texture != 0) + { + + assert(index.x() < sbtData.texture_coords_count); + assert(index.y() < sbtData.texture_coords_count); + assert(index.z() < sbtData.texture_coords_count); + + const Vec2f &uvA = sbtData.texture_coords[index.x()]; + const Vec2f &uvB = sbtData.texture_coords[index.y()]; + const Vec2f &uvC = sbtData.texture_coords[index.z()]; + + Vec2f uv = (1 - u - v) * uvA + u * uvB + v * uvC; + + intensity = tex2D(sbtData.texture, uv[0], uv[1]); + } + + saveRayResult(&hitWorld, &origin, intensity, objectID); } extern "C" __global__ void __miss__() diff --git a/src/repr.hpp b/src/repr.hpp index 756ba668..7a58c580 100644 --- a/src/repr.hpp +++ b/src/repr.hpp @@ -17,6 +17,18 @@ #include #include +template<> +struct fmt::formatter +{ + template + constexpr auto parse(ParseContext& ctx) { return ctx.begin(); } + + template + auto format(const rgl_vec2f& v, FormatContext& ctx) { + return fmt::format_to(ctx.out(), "({}, {})", v.value[0], v.value[1]); + } +}; + template<> struct fmt::formatter { diff --git a/src/scene/Entity.cpp b/src/scene/Entity.cpp index 74e1ce8a..ed318d42 100644 --- a/src/scene/Entity.cpp +++ b/src/scene/Entity.cpp @@ -51,3 +51,11 @@ OptixInstance Entity::getIAS(int idx) transform.toRaw(instance.transform); return instance; } + +void Entity::setIntensityTexture(std::shared_ptr texture) +{ + intensityTexture = texture; + if (auto activeScene = scene.lock()) { + activeScene->requestSBTRebuild(); + } +} \ No newline at end of file diff --git a/src/scene/Entity.hpp b/src/scene/Entity.hpp index d717cacc..248a101a 100644 --- a/src/scene/Entity.hpp +++ b/src/scene/Entity.hpp @@ -30,12 +30,18 @@ struct Entity : APIObject // TODO(prybicki): low-prio optimization: do not rebuild whole IAS if only transform changed void setTransform(Mat3x4f newTransform); + void setId(int newId); int getId() const { return id; } + + void setIntensityTexture(std::shared_ptr texture); + OptixInstance getIAS(int idx); std::shared_ptr mesh; + std::shared_ptr intensityTexture = nullptr; std::weak_ptr scene; + private: Mat3x4f transform; int id; diff --git a/src/scene/Mesh.cpp b/src/scene/Mesh.cpp index cf3db715..47cece60 100644 --- a/src/scene/Mesh.cpp +++ b/src/scene/Mesh.cpp @@ -29,8 +29,9 @@ Mesh::Mesh(const Vec3f *vertices, size_t vertexCount, const Vec3i *indices, size void Mesh::updateVertices(const Vec3f *vertices, std::size_t vertexCount) { if (dVertices.getElemCount() != vertexCount) { - auto msg = fmt::format("Invalid argument: cannot update vertices because vertex counts do not match: old={}, new={}", - dVertices.getElemCount(), vertexCount); + auto msg = fmt::format( + "Invalid argument: cannot update vertices because vertex counts do not match: old={}, new={}", + dVertices.getElemCount(), vertexCount); throw std::invalid_argument(msg); } dVertices.copyFromHost(vertices, vertexCount); @@ -85,36 +86,36 @@ OptixTraversableHandle Mesh::buildGAS() vertexBuffers[0] = dVertices.readDeviceRaw(); buildInput = { - .type = OPTIX_BUILD_INPUT_TYPE_TRIANGLES, - .triangleArray = { - .vertexBuffers = vertexBuffers, - .numVertices = static_cast(dVertices.getElemCount()), - .vertexFormat = OPTIX_VERTEX_FORMAT_FLOAT3, - .vertexStrideInBytes = sizeof(decltype(dVertices)::ValueType), - .indexBuffer = dIndices.readDeviceRaw(), - .numIndexTriplets = static_cast(dIndices.getElemCount()), - .indexFormat = OPTIX_INDICES_FORMAT_UNSIGNED_INT3, - .indexStrideInBytes = sizeof(decltype(dIndices)::ValueType), - .flags = &triangleInputFlags, - .numSbtRecords = 1, - .sbtIndexOffsetBuffer = 0, - .sbtIndexOffsetSizeInBytes = 0, - .sbtIndexOffsetStrideInBytes = 0, - } + .type = OPTIX_BUILD_INPUT_TYPE_TRIANGLES, + .triangleArray = { + .vertexBuffers = vertexBuffers, + .numVertices = static_cast(dVertices.getElemCount()), + .vertexFormat = OPTIX_VERTEX_FORMAT_FLOAT3, + .vertexStrideInBytes = sizeof(decltype(dVertices)::ValueType), + .indexBuffer = dIndices.readDeviceRaw(), + .numIndexTriplets = static_cast(dIndices.getElemCount()), + .indexFormat = OPTIX_INDICES_FORMAT_UNSIGNED_INT3, + .indexStrideInBytes = sizeof(decltype(dIndices)::ValueType), + .flags = &triangleInputFlags, + .numSbtRecords = 1, + .sbtIndexOffsetBuffer = 0, + .sbtIndexOffsetSizeInBytes = 0, + .sbtIndexOffsetStrideInBytes = 0, + } }; buildOptions = { - .buildFlags = OPTIX_BUILD_FLAG_PREFER_FAST_TRACE - | OPTIX_BUILD_FLAG_ALLOW_UPDATE, - // | OPTIX_BUILD_FLAG_ALLOW_COMPACTION, // Temporarily disabled - .operation = OPTIX_BUILD_OPERATION_BUILD + .buildFlags = OPTIX_BUILD_FLAG_PREFER_FAST_TRACE + | OPTIX_BUILD_FLAG_ALLOW_UPDATE, + // | OPTIX_BUILD_FLAG_ALLOW_COMPACTION, // Temporarily disabled + .operation = OPTIX_BUILD_OPERATION_BUILD }; scratchpad.resizeToFit(buildInput, buildOptions); // OptixAccelEmitDesc emitDesc = { - // .result = scratchpad.dCompactedSize.readDeviceRaw(), - // .type = OPTIX_PROPERTY_TYPE_COMPACTED_SIZE, + // .result = scratchpad.dCompactedSize.readDeviceRaw(), + // .type = OPTIX_PROPERTY_TYPE_COMPACTED_SIZE, // }; OptixTraversableHandle gasHandle; @@ -138,3 +139,22 @@ OptixTraversableHandle Mesh::buildGAS() gasNeedsUpdate = false; return gasHandle; } + +void Mesh::setTexCoords(const Vec2f *texCoords, std::size_t texCoordCount) +{ + if (texCoordCount != dVertices.getElemCount()) { + auto msg = fmt::format( + "Invalid argument: cannot set texture coordinates because vertex count do not match with texture coordinates count: vertices={}, texture_coords={}", + dVertices.getElemCount(), texCoordCount); + throw std::invalid_argument(msg); + } + + // TODO is dat valid approach? + if(!dTextureCoords.has_value()) + { + dTextureCoords.emplace(); + } + + dTextureCoords->copyFromHost(texCoords, texCoordCount); + gasNeedsUpdate = true; +} diff --git a/src/scene/Mesh.hpp b/src/scene/Mesh.hpp index c826420c..2f06b507 100644 --- a/src/scene/Mesh.hpp +++ b/src/scene/Mesh.hpp @@ -31,12 +31,15 @@ struct Mesh : APIObject { - void updateVertices(const Vec3f *vertices, std::size_t vertexCount); + void updateVertices(const Vec3f* vertices, std::size_t vertexCount); OptixTraversableHandle getGAS(); + int getVertexCount() const { return dVertices.getElemCount(); } + + void setTexCoords(const Vec2f *texCoords, std::size_t texCoordCount); private: - Mesh(const Vec3f *vertices, std::size_t vertexCount, - const Vec3i *indices, std::size_t indexCount); + Mesh(const Vec3f* vertices, std::size_t vertexCount, + const Vec3i* indices, std::size_t indexCount); OptixTraversableHandle buildGAS(); void updateGAS(); @@ -49,11 +52,11 @@ struct Mesh : APIObject std::optional cachedGAS; DeviceBuffer dVertices; DeviceBuffer dIndices; + std::optional> dTextureCoords; // Shared between buildGAS() and updateGAS() OptixBuildInput buildInput; CUdeviceptr vertexBuffers[1]; unsigned triangleInputFlags; OptixAccelBuildOptions buildOptions; - }; diff --git a/src/scene/Scene.cpp b/src/scene/Scene.cpp index bf97bf75..de1682a6 100644 --- a/src/scene/Scene.cpp +++ b/src/scene/Scene.cpp @@ -14,6 +14,7 @@ #include #include +#include API_OBJECT_INSTANCE(Scene); @@ -86,6 +87,9 @@ OptixShaderBindingTable Scene::buildSBT() .vertex_count = mesh->dVertices.getElemCount(), .index_count = mesh->dIndices.getElemCount(), .entity_id = entity->getId(), + .texture_coords = mesh->dTextureCoords.has_value() ? mesh->dTextureCoords.value().readDevice() : nullptr, + .texture_coords_count = (mesh->dTextureCoords.has_value()) ? (mesh->dTextureCoords.value().getElemCount()) : 0, + .texture = entity->intensityTexture != nullptr ? entity->intensityTexture->getTextureObject() : 0, }; } dHitgroupRecords.copyFromHost(hHitgroupRecords); diff --git a/src/scene/Texture.cpp b/src/scene/Texture.cpp new file mode 100644 index 00000000..cf344668 --- /dev/null +++ b/src/scene/Texture.cpp @@ -0,0 +1,87 @@ +// Copyright 2023 Robotec.AI +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include "RGLFields.hpp" + +API_OBJECT_INSTANCE(Texture); + +Texture::Texture(const void* texels, int width, int height) try : + resolution(width, height) + { + createTextureObject(texels, width, height); + } + catch (...) + { + cleanup(); + throw; + } + +void Texture::createTextureObject(const void* texels, int width, int height) +{ + cudaResourceDesc res_desc = {}; + + int32_t numComponents = 1; + + cudaChannelFormatDesc channel_desc = cudaCreateChannelDesc(); + + int32_t pitch = width * numComponents * sizeof(TextureTexelFormat); + + // TODO prybicki + // Should we leave it like this, or add new copiers in DeivceBuffer.hpp? + // Current copyFromHost and ensureDeviceCanFit are not working with cudaArray_t + CHECK_CUDA(cudaMallocArray(&dPixelArray, &channel_desc, width, height)); + + CHECK_CUDA(cudaMemcpy2DToArray( + dPixelArray, + 0, 0, + texels, + pitch, pitch, height, + cudaMemcpyHostToDevice)); + + res_desc.resType = cudaResourceTypeArray; + res_desc.res.array.array = dPixelArray; + + cudaTextureDesc tex_desc = {}; + + tex_desc.addressMode[0] = cudaAddressModeWrap; + tex_desc.addressMode[1] = cudaAddressModeWrap; + tex_desc.filterMode = cudaFilterModePoint; + tex_desc.readMode = cudaReadModeElementType; + tex_desc.normalizedCoords = 1; + tex_desc.maxAnisotropy = 1; + tex_desc.maxMipmapLevelClamp = 99; + tex_desc.minMipmapLevelClamp = 0; + tex_desc.mipmapFilterMode = cudaFilterModePoint; + tex_desc.borderColor[0] = 1.0f; + + CHECK_CUDA(cudaCreateTextureObject(&dTextureObject, &res_desc, &tex_desc, nullptr)); +} + +Texture::~Texture() +{ + cleanup(); +} + +void Texture::cleanup() +{ + cudaDestroyTextureObject(dTextureObject); + dTextureObject = 0; + if(dPixelArray != nullptr) + { + cudaFreeArray(dPixelArray); + dPixelArray = nullptr; + } +} diff --git a/src/scene/Texture.hpp b/src/scene/Texture.hpp new file mode 100644 index 00000000..d34e208f --- /dev/null +++ b/src/scene/Texture.hpp @@ -0,0 +1,51 @@ +// Copyright 2023 Robotec.AI +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +#include +#include +#include + +struct Texture : APIObject { + +public: + + ~Texture(); + + Vec2i getResolution() const { return resolution; } + + size_t getWidth() const { return resolution.x(); } + + size_t getHeight() const { return resolution.y(); } + + cudaTextureObject_t getTextureObject() const { return dTextureObject; } + + +private: + + Texture(const void* texels, int width, int height); + + Texture(const Texture&) = delete; // non construction-copyable + Texture &operator=(const Texture&) = delete; // non copyable + + void createTextureObject(const void* texels, int width, int height); + + void cleanup(); + + friend APIObject; + Vec2i resolution{-1}; + + cudaTextureObject_t dTextureObject; + cudaArray_t dPixelArray; +}; \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 72430356..58987952 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -10,6 +10,7 @@ set(RGL_TEST_FILES # src/features/transforms.cpp # src/features/pcdOutput.cpp src/testMat3x4f.cpp + src/scene/textureTest.cpp src/graph/nodes/FromArrayPointsNodeTest.cpp src/graph/nodes/TransformPointsNodeTest.cpp src/scene/entityIdTest.cpp diff --git a/test/include/models.hpp b/test/include/models.hpp index bad8d6d1..4d7371ce 100644 --- a/test/include/models.hpp +++ b/test/include/models.hpp @@ -22,6 +22,17 @@ static rgl_vec3f cubeVerticesX2[] = { {-2, 2, 2} }; +static rgl_vec2f cubeUVs[] = { + {0.0f, 0.0f}, + {1.0f, 0.0f}, + {1.0f, 1.0f}, + {0.0f, 1.0f}, + {0.0f, 0.0f}, + {1.0f, 0.0f}, + {1.0f, 1.0f}, + {0.0f, 1.0f}, +}; + static rgl_vec3i cubeIndices[] = { {0, 1, 3}, {3, 1, 2}, @@ -34,7 +45,7 @@ static rgl_vec3i cubeIndices[] = { {3, 2, 7}, {7, 2, 6}, {4, 5, 0}, - {0, 5, 1}, + {0, 5, 1} }; #define ARRAY_SIZE(array) (sizeof(array) / sizeof(*array)) diff --git a/test/include/utils.hpp b/test/include/utils.hpp index 018fee52..14432a17 100644 --- a/test/include/utils.hpp +++ b/test/include/utils.hpp @@ -38,7 +38,7 @@ constexpr float EPSILON_F = 1e-6f; constexpr int maxGPUCoresTestCount = 20000; static rgl_mat3x4f identityTestTransform = Mat3x4f::identity().toRGL(); -static rgl_mat3x4f translationTestTransform = Mat3x4f::translation(1, 2, 3).toRGL(); +static rgl_mat3x4f translationTestTransform = Mat3x4f::translation(2, 3, 4).toRGL(); static rgl_mat3x4f rotationTestTransform = Mat3x4f::rotation(10, 30, 45).toRGL(); static rgl_mat3x4f scalingTestTransform = Mat3x4f::scale(1, 2, 3).toRGL(); static rgl_mat3x4f complexTestTransform = Mat3x4f::TRS(Vec3f(1, 2, 3), Vec3f(10, 30, 45)).toRGL(); @@ -165,6 +165,7 @@ static std::string readFileStr(std::filesystem::path path) // } + static rgl_mesh_t loadMesh(std::filesystem::path path) { rgl_mesh_t mesh = nullptr; @@ -173,3 +174,46 @@ static rgl_mesh_t loadMesh(std::filesystem::path path) EXPECT_RGL_SUCCESS(rgl_mesh_create(&mesh, vs.data(), vs.size(), is.data(), is.size())); return mesh; } + +template +static std::vector generateStaticColorTexture(int width, int height, T value) +{ + auto texels = std::vector(width * height); + + for (int i = 0; i < width * height; ++i) + { + texels[i] = (T)value; + } + return texels; +} + +template +static std::vector generateCheckerboardTexture(int width, int height) +{ + // Generate a sample texture with a grid pattern 16x16. + int xGridSize = ceil(width / 16.0f); + int yGridSize = ceil(height / 16.0f); + int xStep = 0; + int yStep = 0; + + auto texels = std::vector(width * height); + + for (int i = 0; i < width; ++i) + { + for (int j = 0; j < height; ++j) + { + texels[i * height + j] = (T)yStep * 0.5f + (T)xStep * 0.5f; + if (j % yGridSize == 0) + { + yStep += yGridSize; + } + } + yStep = 0; + if (i % xGridSize == 0) + { + xStep += xGridSize; + } + } + + return texels; +} \ No newline at end of file diff --git a/test/src/scene/textureTest.cpp b/test/src/scene/textureTest.cpp new file mode 100644 index 00000000..f8843676 --- /dev/null +++ b/test/src/scene/textureTest.cpp @@ -0,0 +1,152 @@ +#include +#include "utils.hpp" +#include "scenes.hpp" +#include "lidars.hpp" + +#ifdef RGL_BUILD_ROS2_EXTENSION +#include +#endif + +constexpr unsigned short UsMaxValue = 255; + +struct TextureTest : public RGLTestWithParam>, public RGLPointTestHelper{}; + +INSTANTIATE_TEST_SUITE_P( + Parametrized, TextureTest, + testing::Combine( + testing::Values(1, 400), + testing::Values(3, 16, 2000), + testing::Values(0, UsMaxValue/2, UsMaxValue) + )); + +TEST_F(TextureTest, rgl_texture_invalid_argument) +{ + rgl_texture_t texture; + std::vector textureRawData; + + auto initializeArgumentsLambda = [&texture, &textureRawData]() { + texture= nullptr; + textureRawData = generateStaticColorTexture(100, 100, 100); + }; + initializeArgumentsLambda(); + EXPECT_RGL_INVALID_ARGUMENT(rgl_texture_create(nullptr, textureRawData.data(), 100, 100), "texture != nullptr"); + initializeArgumentsLambda(); + EXPECT_RGL_INVALID_ARGUMENT(rgl_texture_create(&texture, nullptr, 100, 100), "texels != nullptr"); + initializeArgumentsLambda(); + EXPECT_RGL_INVALID_ARGUMENT(rgl_texture_create(&texture, textureRawData.data(), -1, 100), "width > 0"); + initializeArgumentsLambda(); + EXPECT_RGL_INVALID_ARGUMENT(rgl_texture_create(&texture, textureRawData.data(), 100, 0), "height > 0"); + +} + +TEST_P(TextureTest, rgl_texture_reading) +{ + auto [width, height, value] = GetParam(); + + rgl_texture_t texture= nullptr; + rgl_entity_t entity = nullptr; + rgl_mesh_t mesh = makeCubeMesh(); + auto textureRawData = generateStaticColorTexture(width, height, value); + + EXPECT_RGL_SUCCESS(rgl_texture_create(&texture, textureRawData.data(), width, height)); + EXPECT_RGL_SUCCESS(rgl_mesh_set_texture_coords(mesh, cubeUVs, ARRAY_SIZE(cubeUVs))); + + EXPECT_RGL_SUCCESS(rgl_entity_create(&entity, nullptr, mesh)); + EXPECT_RGL_SUCCESS(rgl_entity_set_intensity_texture(entity, texture)); + + // Create RGL graph pipeline. + rgl_node_t useRaysNode = nullptr, raytraceNode = nullptr, compactNode = nullptr, yieldNode = nullptr; + + std::vector rays = makeLidar3dRays(360, 360, 0.36, 0.36); + + std::vector yieldFields = { + INTENSITY_F32 + }; + + EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&useRaysNode, rays.data(), rays.size())); + EXPECT_RGL_SUCCESS(rgl_node_raytrace(&raytraceNode, nullptr, 1000)); + EXPECT_RGL_SUCCESS(rgl_node_points_compact(&compactNode)); + EXPECT_RGL_SUCCESS(rgl_node_points_yield(&yieldNode, yieldFields.data(), yieldFields.size())); + + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(useRaysNode, raytraceNode)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(raytraceNode, compactNode)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(compactNode, yieldNode)); + + EXPECT_RGL_SUCCESS(rgl_graph_run(raytraceNode)); + + std::vector<::Field::type> outIntensity; + + int32_t outCount, outSizeOf; + EXPECT_RGL_SUCCESS(rgl_graph_get_result_size(yieldNode, INTENSITY_F32, &outCount, &outSizeOf)); + EXPECT_EQ(outSizeOf, getFieldSize(INTENSITY_F32)); + + outIntensity.resize(outCount); + + EXPECT_RGL_SUCCESS(rgl_graph_get_result_data(yieldNode, INTENSITY_F32, outIntensity.data())); + + for (int i = 0; i < outCount; ++i) + { + EXPECT_NEAR(((float)value), outIntensity.at(i), EPSILON_F); + } + +} + +// Use-case test. Create texture, mesh and entity. Set texture to entity and run graph pipeline. +// As a result, we should get the cube with assigned gradient-checkerboard texture. +TEST_P(TextureTest, rgl_texture_use_case) +{ + auto [width, height, value] = GetParam(); + rgl_texture_t texture; + rgl_mesh_t mesh; + rgl_entity_t entity; + + // Create mesh with assigned texture. + auto textureRawData = generateCheckerboardTexture(width, height); + mesh = makeCubeMesh(); + + EXPECT_RGL_SUCCESS(rgl_texture_create(&texture, textureRawData.data(), 256, 128)); + EXPECT_RGL_SUCCESS(rgl_mesh_set_texture_coords(mesh, cubeUVs, 8)); + + EXPECT_RGL_SUCCESS(rgl_entity_create(&entity, nullptr, mesh)); + EXPECT_RGL_SUCCESS(rgl_entity_set_intensity_texture(entity, texture)); + + //Create RGL graph pipeline. + rgl_node_t useRaysNode = nullptr, raytraceNode = nullptr; + + std::vector rays = makeLidar3dRays(360, 360, 0.36, 0.36); + + std::vector yieldFields = { + XYZ_F32, + INTENSITY_F32, + IS_HIT_I32 + }; + + EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&useRaysNode, rays.data(), rays.size())); + EXPECT_RGL_SUCCESS(rgl_node_raytrace(&raytraceNode, nullptr, 1000)); + + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(useRaysNode, raytraceNode)); + + EXPECT_RGL_SUCCESS(rgl_graph_run(raytraceNode)); + + // Infinite loop to publish pointcloud to ROS2 topic for visualization purposes. Uncomment to use. Use wisely. +#ifdef RGL_BUILD_ROS2_EXTENSION + + // rgl_node_t formatNode = nullptr, ros2publishNode = nullptr; + // + // EXPECT_RGL_SUCCESS(rgl_node_points_format(&formatNode, yieldFields.data(), yieldFields.size())); + // EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&ros2publishNode, "pointcloud", "rgl")); + // + // EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(raytraceNode, formatNode)); + // EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(formatNode, ros2publishNode)); + // + // while(true) + // { + // EXPECT_RGL_SUCCESS(rgl_graph_run(raytraceNode)); + // printf("Publishing pointcloud...\n"); + // } + +#endif + + +} + diff --git a/test/src/tapeSurfaceTest.cpp b/test/src/tapeSurfaceTest.cpp index c31e306c..37efd723 100644 --- a/test/src/tapeSurfaceTest.cpp +++ b/test/src/tapeSurfaceTest.cpp @@ -38,6 +38,15 @@ TEST_F(TapeCase, RecordPlayAllCalls) EXPECT_RGL_SUCCESS(rgl_entity_create(&entity, nullptr, mesh)); EXPECT_RGL_SUCCESS(rgl_entity_set_pose(entity, &identityTf)); + rgl_texture_t texture = nullptr; + int width = 1024; + int height = 2048; + auto textureRawData = generateCheckerboardTexture(width, height); + + EXPECT_RGL_SUCCESS(rgl_texture_create(&texture, textureRawData.data(), width, height)); + EXPECT_RGL_SUCCESS(rgl_mesh_set_texture_coords(mesh, cubeUVs, 8)); + EXPECT_RGL_SUCCESS(rgl_entity_set_intensity_texture(entity, texture)); + EXPECT_RGL_SUCCESS(rgl_scene_set_time(nullptr, 1.5 * 1e9)); rgl_node_t useRays = nullptr; @@ -128,6 +137,7 @@ TEST_F(TapeCase, RecordPlayAllCalls) EXPECT_RGL_SUCCESS(rgl_graph_destroy(setRingIds)); EXPECT_RGL_SUCCESS(rgl_entity_destroy(entity)); EXPECT_RGL_SUCCESS(rgl_mesh_destroy(mesh)); + EXPECT_RGL_SUCCESS(rgl_texture_destroy(texture)); EXPECT_RGL_SUCCESS(rgl_cleanup());