diff --git a/applications/reconstruct/CMakeLists.txt b/applications/reconstruct/CMakeLists.txt index 82e2d43d84ddb6a33db19e168d2dff102cf70f0d..82e0182d3a4cea97b8e85dfb29f73342378ea97d 100644 --- a/applications/reconstruct/CMakeLists.txt +++ b/applications/reconstruct/CMakeLists.txt @@ -9,6 +9,7 @@ set(REPSRC src/camera_util.cu src/ray_cast_sdf.cpp src/registration.cpp + src/virtual_source.cpp ) add_executable(ftl-reconstruct ${REPSRC}) diff --git a/applications/reconstruct/include/ftl/ray_cast_sdf.hpp b/applications/reconstruct/include/ftl/ray_cast_sdf.hpp index 5ccb268d45cbbbde4cf753579ff081d627beb83b..6e0796bfc40d7c7854a251caadb507c5bd861fe8 100644 --- a/applications/reconstruct/include/ftl/ray_cast_sdf.hpp +++ b/applications/reconstruct/include/ftl/ray_cast_sdf.hpp @@ -23,23 +23,23 @@ public: static RayCastParams parametersFromConfig(const nlohmann::json& gas) { RayCastParams params; - params.m_width = gas["adapterWidth"].get<unsigned int>(); - params.m_height = gas["adapterHeight"].get<unsigned int>(); + params.m_width = gas["width"].get<unsigned int>(); + params.m_height = gas["height"].get<unsigned int>(); params.m_intrinsics = MatrixConversion::toCUDA(Eigen::Matrix4f()); params.m_intrinsicsInverse = MatrixConversion::toCUDA(Eigen::Matrix4f()); - params.m_minDepth = gas["sensorDepthMin"].get<float>(); - params.m_maxDepth = gas["sensorDepthMax"].get<float>(); + params.m_minDepth = gas["min_depth"].get<float>(); + params.m_maxDepth = gas["max_depth"].get<float>(); params.m_rayIncrement = gas["SDFRayIncrementFactor"].get<float>() * gas["SDFTruncation"].get<float>(); params.m_thresSampleDist = gas["SDFRayThresSampleDistFactor"].get<float>() * params.m_rayIncrement; params.m_thresDist = gas["SDFRayThresDistFactor"].get<float>() * params.m_rayIncrement; params.m_useGradients = gas["SDFUseGradients"].get<bool>(); - params.m_maxNumVertices = gas["hashNumSDFBlocks"].get<unsigned int>() * 6; + //params.m_maxNumVertices = gas["hashNumSDFBlocks"].get<unsigned int>() * 6; return params; } - void render(const ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const DepthCameraData& cameraData, const Eigen::Matrix4f& lastRigidTransform); + void render(const ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const DepthCameraParams& cameraParams, const Eigen::Matrix4f& lastRigidTransform); const RayCastData& getRayCastData(void) { return m_data; @@ -57,7 +57,7 @@ private: void create(const RayCastParams& params); void destroy(void); - void rayIntervalSplatting(const ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const DepthCameraData& cameraData, const Eigen::Matrix4f& lastRigidTransform); // rasterize + void rayIntervalSplatting(const ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const Eigen::Matrix4f& lastRigidTransform); // rasterize RayCastParams m_params; RayCastData m_data; diff --git a/applications/reconstruct/include/ftl/ray_cast_util.hpp b/applications/reconstruct/include/ftl/ray_cast_util.hpp index e35275279d81a36222c7f4178bb08b17153a9468..f3b4a5ec8659073778e667b3ad5edc4366197447 100644 --- a/applications/reconstruct/include/ftl/ray_cast_util.hpp +++ b/applications/reconstruct/include/ftl/ray_cast_util.hpp @@ -59,10 +59,10 @@ struct RayCastData { updateConstantRayCastParams(params); } - __host__ void download(float3 *points, uchar3 *colours, const RayCastParams& params) const { + __host__ void download(int *depth, uchar3 *colours, const RayCastParams& params) const { //printf("Download: %d,%d\n", params.m_width, params.m_height); - //cudaSafeCall(cudaMemcpy(points, d_depth3, sizeof(float3) * params.m_width * params.m_height, cudaMemcpyDeviceToHost)); - cudaSafeCall(cudaMemcpy(colours, d_colors, sizeof(uchar3) * params.m_width * params.m_height, cudaMemcpyDeviceToHost)); + if (depth) cudaSafeCall(cudaMemcpy(depth, d_depth_i, sizeof(int) * params.m_width * params.m_height, cudaMemcpyDeviceToHost)); + if (colours) cudaSafeCall(cudaMemcpy(colours, d_colors, sizeof(uchar3) * params.m_width * params.m_height, cudaMemcpyDeviceToHost)); } __host__ @@ -196,7 +196,7 @@ struct RayCastData { } __device__ - void traverseCoarseGridSimpleSampleAll(const ftl::voxhash::HashData& hash, const DepthCameraData& cameraData, const float3& worldCamPos, const float3& worldDir, const float3& camDir, const int3& dTid, float minInterval, float maxInterval) const + void traverseCoarseGridSimpleSampleAll(const ftl::voxhash::HashData& hash, const float3& worldCamPos, const float3& worldDir, const float3& camDir, const int3& dTid, float minInterval, float maxInterval) const { const RayCastParams& rayCastParams = c_rayCastParams; @@ -236,7 +236,7 @@ struct RayCastData { float depth = alpha / depthToRayLength; // Convert ray length to depth depthToRayLength d_depth[dTid.y*rayCastParams.m_width+dTid.x] = depth; - d_depth3[dTid.y*rayCastParams.m_width+dTid.x] = cameraData.kinectDepthToSkeleton(dTid.x, dTid.y, depth); + d_depth3[dTid.y*rayCastParams.m_width+dTid.x] = DepthCameraData::kinectDepthToSkeleton(dTid.x, dTid.y, depth); d_colors[dTid.y*rayCastParams.m_width+dTid.x] = make_uchar3(color2.x, color2.y, color2.z); if(rayCastParams.m_useGradients) diff --git a/applications/reconstruct/include/ftl/virtual_source.hpp b/applications/reconstruct/include/ftl/virtual_source.hpp new file mode 100644 index 0000000000000000000000000000000000000000..6bc1022f719863a893d0586d6fd7b6e010873f05 --- /dev/null +++ b/applications/reconstruct/include/ftl/virtual_source.hpp @@ -0,0 +1,48 @@ +#pragma once +#ifndef _FTL_RGBD_VIRTUAL_HPP_ +#define _FTL_RGBD_VIRTUAL_HPP_ + +#include <ftl/rgbd_source.hpp> + +class CUDARayCastSDF; + +namespace ftl { +namespace voxhash { + class SceneRep; +} + +namespace rgbd { + +/** + * RGBD source from either a stereo video file with left + right images, or + * direct from two camera devices. A variety of algorithms are included for + * calculating disparity, before converting to depth. Calibration of the images + * is also performed. + */ +class VirtualSource : public RGBDSource { + public: + VirtualSource(nlohmann::json &config, ftl::net::Universe *net); + ~VirtualSource(); + + void setScene(ftl::voxhash::SceneRep *); + + void grab(); + void getRGBD(cv::Mat &rgb, cv::Mat &depth); + bool isReady(); + + static inline RGBDSource *create(nlohmann::json &config, ftl::net::Universe *net) { + return new VirtualSource(config, net); + } + + private: + ftl::voxhash::SceneRep *scene_; + CUDARayCastSDF *rays_; + bool ready_; + cv::Mat rgb_; + cv::Mat depth_; +}; + +} +} + +#endif // _FTL_RGBD_VIRTUAL_HPP_ diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp index 564d0da12746d6001a57630a08d1ba11e3e8699b..9a40bf1ad24e246205c5b02c2e39dc642d6ff030 100644 --- a/applications/reconstruct/src/main.cpp +++ b/applications/reconstruct/src/main.cpp @@ -9,8 +9,8 @@ #include <ftl/configuration.hpp> #include <ftl/depth_camera.hpp> #include <ftl/scene_rep_hash_sdf.hpp> -#include <ftl/ray_cast_sdf.hpp> #include <ftl/rgbd.hpp> +#include <ftl/virtual_source.hpp> // #include <zlib.h> // #include <lz4.h> @@ -23,7 +23,7 @@ #include <opencv2/opencv.hpp> #include <ftl/net/universe.hpp> -#include <ftl/display.hpp> +#include <ftl/rgbd_display.hpp> #include <nlohmann/json.hpp> #include <opencv2/imgproc.hpp> @@ -45,7 +45,7 @@ #endif using ftl::net::Universe; -using ftl::Display; +using ftl::rgbd::Display; using ftl::config; using std::string; using std::vector; @@ -313,41 +313,6 @@ std::map<string, Eigen::Matrix4f> runRegistration(ftl::net::Universe &net, Conta } #endif -template<class T> -Eigen::Matrix<T,4,4> lookAt -( - Eigen::Matrix<T,3,1> const & eye, - Eigen::Matrix<T,3,1> const & center, - Eigen::Matrix<T,3,1> const & up -) -{ - typedef Eigen::Matrix<T,4,4> Matrix4; - typedef Eigen::Matrix<T,3,1> Vector3; - - Vector3 f = (center - eye).normalized(); - Vector3 u = up.normalized(); - Vector3 s = f.cross(u).normalized(); - u = s.cross(f); - - Matrix4 res; - res << s.x(),s.y(),s.z(),-s.dot(eye), - u.x(),u.y(),u.z(),-u.dot(eye), - -f.x(),-f.y(),-f.z(),f.dot(eye), - 0,0,0,1; - - return res; -} - -using MouseAction = std::function<void(int, int, int, int)>; - -static void setMouseAction(const std::string& winName, const MouseAction &action) -{ - cv::setMouseCallback(winName, - [] (int event, int x, int y, int flags, void* userdata) { - (*(MouseAction*)userdata)(event, x, y, flags); - }, (void*)&action); -} - static void run() { Universe net(config["net"]); @@ -381,7 +346,7 @@ static void run() { cam.gpu.alloc(cam.params); //displays.emplace_back(config["display"], src["uri"]); - LOG(INFO) << (string)src["uri"] << " loaded " << cam.params.m_imageWidth; + LOG(INFO) << (string)src["uri"] << " loaded " << cam.params.fx; } } @@ -452,59 +417,23 @@ static void run() { for (auto &input : inputs) { LOG(INFO) << " " + (string) input.source->getConfig()["uri"]; } //vector<PointCloud<PointXYZRGB>::Ptr> clouds(inputs.size()); - Display display_merged(config["display"], "Merged"); // todo - CUDARayCastSDF rays(config["voxelhash"]); + ftl::rgbd::Display display(config["display"]); // todo + ftl::rgbd::VirtualSource *virt = new ftl::rgbd::VirtualSource(config["virtual"], &net); ftl::voxhash::SceneRep scene(config["voxelhash"]); + virt->setScene(&scene); + display.setSource(virt); - cv::Mat colour_array(cv::Size(rays.getRayCastParams().m_width,rays.getRayCastParams().m_height), CV_8UC3); - - // Force window creation - display_merged.render(colour_array); - display_merged.wait(1); unsigned char frameCount = 0; bool paused = false; - float cam_x = 0.0f; - float cam_z = 0.0f; - - Eigen::Vector3f eye(0.0f, 0.0f, 0.0f); - Eigen::Vector3f centre(0.0f, 0.0f, -4.0f); - Eigen::Vector3f up(0,1.0f,0); - Eigen::Vector3f lookPoint(0.0f,0.0f,-4.0f); - Eigen::Matrix4f viewPose; - float lerpSpeed = 0.4f; // Keyboard camera controls - display_merged.onKey([&paused,&eye,¢re](int key) { - LOG(INFO) << "Key = " << key; + display.onKey([&paused](int key) { if (key == 32) paused = !paused; - else if (key == 81 || key == 83) { - // TODO Should rotate around lookAt object, but requires correct depth - Eigen::Quaternion<float> q; q = Eigen::AngleAxis<float>((key == 81) ? 0.01f : -0.01f, Eigen::Vector3f(0.0,1.0f,0.0f)); - eye = (q * (eye - centre)) + centre; - } else if (key == 84 || key == 82) { - float scalar = (key == 84) ? 0.99f : 1.01f; - eye = ((eye - centre) * scalar) + centre; - } }); - // TODO(Nick) Calculate "camera" properties of viewport. - MouseAction mouseact = [&inputs,&lookPoint,&viewPose]( int event, int ux, int uy, int) { - LOG(INFO) << "Mouse " << ux << "," << uy; - if (event == 1) { // click - const float x = ((float)ux-inputs[0].params.m_imageWidth/2) / inputs[0].params.fx; - const float y = ((float)uy-inputs[0].params.m_imageHeight/2) / inputs[0].params.fy; - const float depth = -4.0f; - Eigen::Vector4f camPos(x*depth,y*depth,depth,1.0); - Eigen::Vector4f worldPos = viewPose * camPos; - lookPoint = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]); - LOG(INFO) << "Look at: " << worldPos; - } - }; - ::setMouseAction("Image", mouseact); - int active = inputs.size(); - while (active > 0 && display_merged.active()) { + while (active > 0 && display.active()) { active = 0; if (!paused) { @@ -512,25 +441,16 @@ static void run() { scene.nextFrame(); for (size_t i = 0; i < inputs.size(); i++) { - //if (i == 1) continue; - //Display &display = displays[i]; + // Get the RGB-Depth frame from input RGBDSource *input = inputs[i].source; Mat rgb, depth; - //LOG(INFO) << "GetRGB"; input->getRGBD(rgb,depth); - //if (!display.active()) continue; active += 1; - //clouds[i] = ftl::rgbd::createPointCloud(input); - - //display.render(rgb, depth,input->getParameters()); - //display.render(clouds[i]); - //display.wait(5); - - //LOG(INFO) << "Data size: " << depth.cols << "," << depth.rows; if (depth.cols == 0) continue; + // Must be in RGBA for GPU Mat rgba; cv::cvtColor(rgb,rgba, cv::COLOR_BGR2BGRA); @@ -547,16 +467,7 @@ static void run() { frameCount++; - // Set virtual camera transformation matrix - //Eigen::Affine3f transform(Eigen::Translation3f(cam_x,0.0f,cam_z)); - centre += (lookPoint - centre) * (lerpSpeed * 0.1f); - viewPose = lookAt<float>(eye,centre,up).inverse(); // transform.matrix(); - - // Call GPU renderer and download result into OpenCV mat - rays.render(scene.getHashData(), scene.getHashParams(), inputs[0].gpu, viewPose); - rays.getRayCastData().download(nullptr, (uchar3*)colour_array.data, rays.getRayCastParams()); - display_merged.render(colour_array); - display_merged.wait(1); + display.update(); } } diff --git a/applications/reconstruct/src/ray_cast_sdf.cpp b/applications/reconstruct/src/ray_cast_sdf.cpp index 093d1a2349f5cfd4f260342b7e870301242fbe92..c3723429b914629da77b29c53970dd44f39b01ab 100644 --- a/applications/reconstruct/src/ray_cast_sdf.cpp +++ b/applications/reconstruct/src/ray_cast_sdf.cpp @@ -9,18 +9,17 @@ extern "C" void renderCS( const ftl::voxhash::HashData& hashData, - const RayCastData &rayCastData, - const DepthCameraData &cameraData, + const RayCastData &rayCastData, const RayCastParams &rayCastParams); extern "C" void computeNormals(float4* d_output, float3* d_input, unsigned int width, unsigned int height); extern "C" void convertDepthFloatToCameraSpaceFloat3(float3* d_output, float* d_input, float4x4 intrinsicsInv, unsigned int width, unsigned int height, const DepthCameraData& depthCameraData); extern "C" void resetRayIntervalSplatCUDA(RayCastData& data, const RayCastParams& params); -extern "C" void rayIntervalSplatCUDA(const ftl::voxhash::HashData& hashData, const DepthCameraData& cameraData, +extern "C" void rayIntervalSplatCUDA(const ftl::voxhash::HashData& hashData, const RayCastData &rayCastData, const RayCastParams &rayCastParams); -extern "C" void nickRenderCUDA(const ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const RayCastData &rayCastData, const DepthCameraData &cameraData, const RayCastParams ¶ms); +extern "C" void nickRenderCUDA(const ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const RayCastData &rayCastData, const RayCastParams ¶ms); @@ -37,14 +36,20 @@ void CUDARayCastSDF::destroy(void) //m_rayIntervalSplatting.OnD3D11DestroyDevice(); } -void CUDARayCastSDF::render(const ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const DepthCameraData& cameraData, const Eigen::Matrix4f& lastRigidTransform) +void CUDARayCastSDF::render(const ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const DepthCameraParams& cameraParams, const Eigen::Matrix4f& lastRigidTransform) { - rayIntervalSplatting(hashData, hashParams, cameraData, lastRigidTransform); + updateConstantDepthCameraParams(cameraParams); + //rayIntervalSplatting(hashData, hashParams, lastRigidTransform); //m_data.d_rayIntervalSplatMinArray = m_rayIntervalSplatting.mapMinToCuda(); //m_data.d_rayIntervalSplatMaxArray = m_rayIntervalSplatting.mapMaxToCuda(); - if (hash_render_) nickRenderCUDA(hashData, hashParams, m_data, cameraData, m_params); - else renderCS(hashData, m_data, cameraData, m_params); + m_params.m_numOccupiedSDFBlocks = hashParams.m_numOccupiedBlocks; + m_params.m_viewMatrix = MatrixConversion::toCUDA(lastRigidTransform.inverse()); + m_params.m_viewMatrixInverse = MatrixConversion::toCUDA(lastRigidTransform); + m_data.updateParams(m_params); + + if (hash_render_) nickRenderCUDA(hashData, hashParams, m_data, m_params); + else renderCS(hashData, m_data, m_params); //convertToCameraSpace(cameraData); if (!m_params.m_useGradients) @@ -66,7 +71,7 @@ void CUDARayCastSDF::convertToCameraSpace(const DepthCameraData& cameraData) } } -void CUDARayCastSDF::rayIntervalSplatting(const ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const DepthCameraData& cameraData, const Eigen::Matrix4f& lastRigidTransform) +void CUDARayCastSDF::rayIntervalSplatting(const ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const Eigen::Matrix4f& lastRigidTransform) { if (hashParams.m_numOccupiedBlocks == 0) return; diff --git a/applications/reconstruct/src/ray_cast_sdf.cu b/applications/reconstruct/src/ray_cast_sdf.cu index 9c8b2a7915232dcb2c2151712f44777291eaa6a8..b28bea8acfd2ba415c2e08182a09a1ec58e9bb28 100644 --- a/applications/reconstruct/src/ray_cast_sdf.cu +++ b/applications/reconstruct/src/ray_cast_sdf.cu @@ -13,7 +13,7 @@ //texture<float, cudaTextureType2D, cudaReadModeElementType> rayMinTextureRef; //texture<float, cudaTextureType2D, cudaReadModeElementType> rayMaxTextureRef; -__global__ void renderKernel(ftl::voxhash::HashData hashData, RayCastData rayCastData, DepthCameraData cameraData) +__global__ void renderKernel(ftl::voxhash::HashData hashData, RayCastData rayCastData) { const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x; const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y; @@ -26,7 +26,7 @@ __global__ void renderKernel(ftl::voxhash::HashData hashData, RayCastData rayCas rayCastData.d_normals[y*rayCastParams.m_width+x] = make_float4(MINF,MINF,MINF,MINF); rayCastData.d_colors[y*rayCastParams.m_width+x] = make_uchar3(0,0,0); - float3 camDir = normalize(cameraData.kinectProjToCamera(x, y, 1.0f)); + float3 camDir = normalize(DepthCameraData::kinectProjToCamera(x, y, 1.0f)); float3 worldCamPos = rayCastParams.m_viewMatrixInverse * make_float3(0.0f, 0.0f, 0.0f); float4 w = rayCastParams.m_viewMatrixInverse * make_float4(camDir, 0.0f); float3 worldDir = normalize(make_float3(w.x, w.y, w.z)); @@ -50,11 +50,11 @@ __global__ void renderKernel(ftl::voxhash::HashData hashData, RayCastData rayCas // printf("ERROR (%d,%d): [ %f, %f ]\n", x, y, minInterval, maxInterval); //} - rayCastData.traverseCoarseGridSimpleSampleAll(hashData, cameraData, worldCamPos, worldDir, camDir, make_int3(x,y,1), minInterval, maxInterval); + rayCastData.traverseCoarseGridSimpleSampleAll(hashData, worldCamPos, worldDir, camDir, make_int3(x,y,1), minInterval, maxInterval); } } -extern "C" void renderCS(const ftl::voxhash::HashData& hashData, const RayCastData &rayCastData, const DepthCameraData &cameraData, const RayCastParams &rayCastParams) +extern "C" void renderCS(const ftl::voxhash::HashData& hashData, const RayCastData &rayCastData, const RayCastParams &rayCastParams) { const dim3 gridSize((rayCastParams.m_width + T_PER_BLOCK - 1)/T_PER_BLOCK, (rayCastParams.m_height + T_PER_BLOCK - 1)/T_PER_BLOCK); @@ -66,7 +66,7 @@ extern "C" void renderCS(const ftl::voxhash::HashData& hashData, const RayCastDa //printf("Ray casting render...\n"); - renderKernel<<<gridSize, blockSize>>>(hashData, rayCastData, cameraData); + renderKernel<<<gridSize, blockSize>>>(hashData, rayCastData); #ifdef _DEBUG cudaSafeCall(cudaDeviceSynchronize()); @@ -78,7 +78,7 @@ extern "C" void renderCS(const ftl::voxhash::HashData& hashData, const RayCastDa // Nicks render approach //////////////////////////////////////////////////////////////////////////////// -__global__ void clearDepthKernel(ftl::voxhash::HashData hashData, RayCastData rayCastData, DepthCameraData cameraData) +__global__ void clearDepthKernel(ftl::voxhash::HashData hashData, RayCastData rayCastData) { const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x; const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y; @@ -169,7 +169,7 @@ __device__ inline void trilinearInterp(const ftl::voxhash::HashData &hashData, c colour = make_uchar3(colorFloat); } -__global__ void nickRenderKernel(ftl::voxhash::HashData hashData, RayCastData rayCastData, DepthCameraData cameraData, RayCastParams params) { +__global__ void nickRenderKernel(ftl::voxhash::HashData hashData, RayCastData rayCastData, RayCastParams params) { // TODO(Nick) Reduce bank conflicts by aligning these __shared__ ftl::voxhash::Voxel voxels[SDF_BLOCK_BUFFER]; __shared__ ftl::voxhash::HashEntry blocks[8]; @@ -295,7 +295,7 @@ __global__ void nickRenderKernel(ftl::voxhash::HashData hashData, RayCastData ra if (!is_surface) return; const float3 camPos = params.m_viewMatrix * worldPos; - const float2 screenPosf = cameraData.cameraToKinectScreenFloat(camPos); + const float2 screenPosf = DepthCameraData::cameraToKinectScreenFloat(camPos); const uint2 screenPos = make_uint2(make_int2(screenPosf)); // + make_float2(0.5f, 0.5f) /*if (screenPos.x < params.m_width && screenPos.y < params.m_height && @@ -316,7 +316,7 @@ __global__ void nickRenderKernel(ftl::voxhash::HashData hashData, RayCastData ra for (int x=0; x<pixsizeX; x++) { // TODO(Nick) Within a window, check pixels that have same voxel id // Then trilinear interpolate between current voxel and neighbors. - const float3 pixelWorldPos = params.m_viewMatrixInverse * cameraData.kinectDepthToSkeleton(screenPos.x+x,screenPos.y+y, camPos.z); + const float3 pixelWorldPos = params.m_viewMatrixInverse * DepthCameraData::kinectDepthToSkeleton(screenPos.x+x,screenPos.y+y, camPos.z); const float3 posInVoxel = (pixelWorldPos - worldPos) / make_float3(c_hashParams.m_virtualVoxelSize,c_hashParams.m_virtualVoxelSize,c_hashParams.m_virtualVoxelSize); if (posInVoxel.x >= 1.0f || posInVoxel.y >= 1.0f || posInVoxel.z >= 1.0f) { @@ -348,19 +348,19 @@ __global__ void nickRenderKernel(ftl::voxhash::HashData hashData, RayCastData ra } } -extern "C" void nickRenderCUDA(const ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const RayCastData &rayCastData, const DepthCameraData &cameraData, const RayCastParams ¶ms) +extern "C" void nickRenderCUDA(const ftl::voxhash::HashData& hashData, const ftl::voxhash::HashParams& hashParams, const RayCastData &rayCastData, const RayCastParams ¶ms) { const dim3 clear_gridSize((params.m_width + T_PER_BLOCK - 1)/T_PER_BLOCK, (params.m_height + T_PER_BLOCK - 1)/T_PER_BLOCK); const dim3 clear_blockSize(T_PER_BLOCK, T_PER_BLOCK); - clearDepthKernel<<<clear_gridSize, clear_blockSize>>>(hashData, rayCastData, cameraData); + clearDepthKernel<<<clear_gridSize, clear_blockSize>>>(hashData, rayCastData); const unsigned int threadsPerBlock = SDF_BLOCK_SIZE*SDF_BLOCK_SIZE*SDF_BLOCK_SIZE; const dim3 gridSize(hashParams.m_numOccupiedBlocks, 1); const dim3 blockSize(threadsPerBlock, 1); if (hashParams.m_numOccupiedBlocks > 0) { //this guard is important if there is no depth in the current frame (i.e., no blocks were allocated) - nickRenderKernel << <gridSize, blockSize >> >(hashData, rayCastData, cameraData, params); + nickRenderKernel << <gridSize, blockSize >> >(hashData, rayCastData, params); } cudaSafeCall( cudaGetLastError() ); diff --git a/applications/reconstruct/src/virtual_source.cpp b/applications/reconstruct/src/virtual_source.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7b7bd8299c1a6aed299a48fe00797999f595700f --- /dev/null +++ b/applications/reconstruct/src/virtual_source.cpp @@ -0,0 +1,61 @@ +#include <ftl/virtual_source.hpp> +#include <ftl/depth_camera.hpp> +#include <ftl/scene_rep_hash_sdf.hpp> +#include <ftl/ray_cast_sdf.hpp> + +#include <glog/logging.h> + +using ftl::rgbd::VirtualSource; + +VirtualSource::VirtualSource(nlohmann::json &config, ftl::net::Universe *net) + : RGBDSource(config, net) { + rays_ = new CUDARayCastSDF(config); + scene_ = nullptr; + + params_.fx = config.value("focal", 430.0f); + params_.fy = params_.fx; + params_.width = config.value("width", 640); + params_.height = config.value("height", 480); + params_.cx = params_.width / 2; + params_.cy = params_.height / 2; + params_.maxDepth = config.value("max_depth", 10.0f); + params_.minDepth = config.value("min_depth", 0.1f); + + rgb_ = cv::Mat(cv::Size(params_.width,params_.height), CV_8UC3); + // init depth +} + +VirtualSource::~VirtualSource() { + if (scene_) delete scene_; + if (rays_) delete rays_; +} + +void VirtualSource::setScene(ftl::voxhash::SceneRep *scene) { + scene_ = scene; +} + +void VirtualSource::grab() { + if (scene_) { + DepthCameraParams params; + params.fx = params_.fx; + params.fy = params_.fy; + params.mx = params_.cx; + params.my = params_.cy; + params.m_imageWidth = params_.width; + params.m_imageHeight = params_.height; + params.m_sensorDepthWorldMin = params_.minDepth; + params.m_sensorDepthWorldMax = params_.maxDepth; + + rays_->render(scene_->getHashData(), scene_->getHashParams(), params, getPose()); + rays_->getRayCastData().download(nullptr, (uchar3*)rgb_.data, rays_->getRayCastParams()); + } +} + +void VirtualSource::getRGBD(cv::Mat &rgb, cv::Mat &depth) { + rgb_.copyTo(rgb); + depth_.copyTo(depth); +} + +bool VirtualSource::isReady() { + +} diff --git a/components/renderers/cpp/CMakeLists.txt b/components/renderers/cpp/CMakeLists.txt index e603f6740f6313f2f699e3e0bf9ef78774ab464d..33f910ca0342096bb551b430374776a86de89b2f 100644 --- a/components/renderers/cpp/CMakeLists.txt +++ b/components/renderers/cpp/CMakeLists.txt @@ -1,5 +1,6 @@ add_library(ftlrender src/display.cpp + src/rgbd_display.cpp ) # These cause errors in CI build and are being removed from PCL in newer versions @@ -10,6 +11,6 @@ target_include_directories(ftlrender PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> $<INSTALL_INTERFACE:include> PRIVATE src) -target_link_libraries(ftlrender ftlcommon ftlnet Eigen3::Eigen Threads::Threads glog::glog ${OpenCV_LIBS} ${PCL_LIBRARIES}) +target_link_libraries(ftlrender ftlrgbd ftlcommon ftlnet Eigen3::Eigen Threads::Threads glog::glog ${OpenCV_LIBS} ${PCL_LIBRARIES}) #ADD_SUBDIRECTORY(test) diff --git a/components/renderers/cpp/include/ftl/rgbd_display.hpp b/components/renderers/cpp/include/ftl/rgbd_display.hpp new file mode 100644 index 0000000000000000000000000000000000000000..fead71e645cefaccfbed28fd289541920fb547c0 --- /dev/null +++ b/components/renderers/cpp/include/ftl/rgbd_display.hpp @@ -0,0 +1,47 @@ +#ifndef _FTL_RGBD_DISPLAY_HPP_ +#define _FTL_RGBD_DISPLAY_HPP_ + +#include <nlohmann/json.hpp> +#include <ftl/rgbd_source.hpp> + +using MouseAction = std::function<void(int, int, int, int)>; + +namespace ftl { +namespace rgbd { + +class Display : public ftl::Configurable { + public: + explicit Display(nlohmann::json &); + Display(nlohmann::json &, RGBDSource *); + ~Display(); + + void setSource(RGBDSource *src) { source_ = src; } + void update(); + + bool active() const { return active_; } + + void onKey(std::function<void(int)> h) { key_handlers_.push_back(h); } + + void wait(int ms); + + private: + RGBDSource *source_; + std::string name_; + std::vector<std::function<void(int)>> key_handlers_; + Eigen::Vector3f eye_; + Eigen::Vector3f centre_; + Eigen::Vector3f up_; + Eigen::Vector3f lookPoint_; + float lerpSpeed_; + bool active_; + MouseAction mouseaction_; + + static int viewcount__; + + void init(); +}; + +} +} + +#endif // _FTL_RGBD_DISPLAY_HPP_ diff --git a/components/renderers/cpp/src/display.cpp b/components/renderers/cpp/src/display.cpp index 88ad2b18344c75d5609a1817bd1d7daf6d99befa..092872a58608d6514fdff2d1005b16beceec51c5 100644 --- a/components/renderers/cpp/src/display.cpp +++ b/components/renderers/cpp/src/display.cpp @@ -18,6 +18,8 @@ Display::Display(nlohmann::json &config, std::string name) : config_(config) { window_->setBackgroundColor(cv::viz::Color::white()); #endif // HAVE_VIZ + cv::namedWindow("Image", cv::WINDOW_KEEPRATIO); + #if defined HAVE_PCL if (config.value("points", false)) { pclviz_ = pcl::visualization::PCLVisualizer::Ptr(new pcl::visualization::PCLVisualizer ("FTL Cloud: " + name)); @@ -222,7 +224,6 @@ bool Display::render(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr pc) { #endif // HAVE_PCL bool Display::render(const cv::Mat &img, style_t s) { if (s == STYLE_NORMAL) { - cv::namedWindow("Image", cv::WINDOW_KEEPRATIO); cv::imshow("Image", img); } else if (s = STYLE_DISPARITY) { Mat idepth; diff --git a/components/renderers/cpp/src/rgbd_display.cpp b/components/renderers/cpp/src/rgbd_display.cpp new file mode 100644 index 0000000000000000000000000000000000000000..4eeedd0e9ec76aeb4c14167d8ed097a661504f5f --- /dev/null +++ b/components/renderers/cpp/src/rgbd_display.cpp @@ -0,0 +1,133 @@ +#include <ftl/rgbd_display.hpp> +#include <opencv2/opencv.hpp> + +using ftl::rgbd::RGBDSource; +using ftl::rgbd::Display; +using std::string; +using cv::Mat; + +int Display::viewcount__ = 0; + +template<class T> +Eigen::Matrix<T,4,4> lookAt +( + Eigen::Matrix<T,3,1> const & eye, + Eigen::Matrix<T,3,1> const & center, + Eigen::Matrix<T,3,1> const & up +) +{ + typedef Eigen::Matrix<T,4,4> Matrix4; + typedef Eigen::Matrix<T,3,1> Vector3; + + Vector3 f = (center - eye).normalized(); + Vector3 u = up.normalized(); + Vector3 s = f.cross(u).normalized(); + u = s.cross(f); + + Matrix4 res; + res << s.x(),s.y(),s.z(),-s.dot(eye), + u.x(),u.y(),u.z(),-u.dot(eye), + -f.x(),-f.y(),-f.z(),f.dot(eye), + 0,0,0,1; + + return res; +} + +static void setMouseAction(const std::string& winName, const MouseAction &action) +{ + cv::setMouseCallback(winName, + [] (int event, int x, int y, int flags, void* userdata) { + (*(MouseAction*)userdata)(event, x, y, flags); + }, (void*)&action); +} + +Display::Display(nlohmann::json &config) : ftl::Configurable(config) { + name_ = config.value("name", string("View [")+std::to_string(viewcount__)+string("]")); + viewcount__++; + + init(); +} + +Display::Display(nlohmann::json &config, RGBDSource *source) + : ftl::Configurable(config) { + name_ = config.value("name", string("View [")+std::to_string(viewcount__)+string("]")); + viewcount__++; + init(); +} + +Display::~Display() { + +} + +void Display::init() { + active_ = true; + source_ = nullptr; + cv::namedWindow(name_, cv::WINDOW_KEEPRATIO); + + eye_ = Eigen::Vector3f(0.0f, 0.0f, 0.0f); + centre_ = Eigen::Vector3f(0.0f, 0.0f, -4.0f); + up_ = Eigen::Vector3f(0,1.0f,0); + lookPoint_ = Eigen::Vector3f(0.0f,0.0f,-4.0f); + lerpSpeed_ = 0.4f; + + // Keyboard camera controls + onKey([this](int key) { + LOG(INFO) << "Key = " << key; + if (key == 81 || key == 83) { + // TODO Should rotate around lookAt object, but requires correct depth + Eigen::Quaternion<float> q; q = Eigen::AngleAxis<float>((key == 81) ? 0.01f : -0.01f, Eigen::Vector3f(0.0,1.0f,0.0f)); + eye_ = (q * (eye_ - centre_)) + centre_; + } else if (key == 84 || key == 82) { + float scalar = (key == 84) ? 0.99f : 1.01f; + eye_ = ((eye_ - centre_) * scalar) + centre_; + } + }); + + // TODO(Nick) Calculate "camera" properties of viewport. + mouseaction_ = [this]( int event, int ux, int uy, int) { + LOG(INFO) << "Mouse " << ux << "," << uy; + if (event == 1 && source_) { // click + const auto params = source_->getParameters(); + const float x = ((float)ux-params.width/2) / params.fx; + const float y = ((float)uy-params.height/2) / params.fy; + const float depth = -4.0f; + Eigen::Vector4f camPos(x*depth,y*depth,depth,1.0); + Eigen::Vector4f worldPos = source_->getPose() * camPos; + lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]); + LOG(INFO) << "Look at: " << worldPos; + } + }; + ::setMouseAction(name_, mouseaction_); +} + +void Display::wait(int ms) { + while (true) { + int key = cv::waitKey(ms); + + if(key == 27) { + // exit if ESC is pressed + active_ = false; + } else if (key == -1) { + return; + } else { + ms = 1; + for (auto &h : key_handlers_) { + h(key); + } + } + } +} + +void Display::update() { + if (!source_) return; + + centre_ += (lookPoint_ - centre_) * (lerpSpeed_ * 0.1f); + Eigen::Matrix4f viewPose = lookAt<float>(eye_,centre_,up_).inverse(); + source_->setPose(viewPose); + + Mat rgb, depth; + source_->grab(); + source_->getRGBD(rgb, depth); + cv::imshow(name_, rgb); + wait(1); +}