From 47c610cb4d787ec5f39069272c098b7f1311d069 Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Tue, 28 Jun 2016 17:51:52 +0200 Subject: [PATCH 01/21] Add bin grabber in order to read raw binary recordings from the Kinect --- CMakeLists.txt | 22 ++++ kfusion/include/io/bin_grabber.hpp | 127 +++++++++++++++++++ kfusion/src/bin_grabber.cpp | 196 +++++++++++++++++++++++++++++ 3 files changed, 345 insertions(+) create mode 100644 kfusion/include/io/bin_grabber.hpp create mode 100644 kfusion/src/bin_grabber.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index d2d34e3..80b5886 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,28 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake/Modules/") include(cmake/Utils.cmake) include(cmake/Targets.cmake) +# Find OpenMP +# the gcc-4.2.1 coming with MacOS X is not compatible with the OpenMP pragmas we use, so disabling OpenMP for it +if((NOT (${CMAKE_SYSTEM_NAME} MATCHES "Darwin")) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1) OR (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")) + find_package(OpenMP) +endif() +if(OPENMP_FOUND) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + message (STATUS "Found OpenMP") + if(MSVC90 OR MSVC10) + if(MSVC90) + set(OPENMP_DLL VCOMP90) + elseif(MSVC10) + set(OPENMP_DLL VCOMP100) + endif(MSVC90) + set(CMAKE_SHARED_LINKER_FLAGS_DEBUG "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} /DELAYLOAD:${OPENMP_DLL}D.dll") + set(CMAKE_SHARED_LINKER_FLAGS_RELEASE "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} /DELAYLOAD:${OPENMP_DLL}.dll") + endif(MSVC90 OR MSVC10) +else(OPENMP_FOUND) + message (STATUS "Not found OpenMP") +endif() + # ---[ find dependencies find_package(OpenCV 2.4.8 REQUIRED COMPONENTS core viz highgui) find_package(CUDA 5.0 REQUIRED) diff --git a/kfusion/include/io/bin_grabber.hpp b/kfusion/include/io/bin_grabber.hpp new file mode 100644 index 0000000..c611e4d --- /dev/null +++ b/kfusion/include/io/bin_grabber.hpp @@ -0,0 +1,127 @@ +/** + * @file bin_grabber.hpp + * @brief Bin Grabber for PCL KinFu app + * + * @author Gabriel Cuendet + * @date 21/06/2016 + * Copyright (c) 2016 Gabriel Cuendet. All rights reserved. + */ + +#ifndef __KFUSION_BIN_SOURCE__ +#define __KFUSION_BIN_SOURCE__ + +#include +#include +#include +#include + +namespace kfusion { +class KF_EXPORTS BinSource { + public: + typedef kfusion::PixelRGB RGB24; + + struct KinectData { + /**< Depth map width */ + int depth_frame_width; + /**< Depth map height */ + int depth_frame_height; + /**< Size in bytes of one frame in the binary file */ + int depth_block_size; + /**< Array of memory for the depth map */ + std::vector depth_frame; + /**< RGB Image width */ + int rgb_frame_width; + /**< RGB Image height */ + int rgb_frame_height; + /**< Size in bytes of one frame in the binary file */ + int rgb_block_size; + /**< Array of memory for the rgb image */ + std::vector rgb_frame; + + /** + * @name KinectData + * @fn KinectData(void) + * @brief Default constructor + */ + KinectData(void) : depth_frame_width(0), depth_frame_height(0), + depth_frame(0), rgb_frame_width(0), rgb_frame_height(0), rgb_frame(0) {} + + /** + * @name ~KinectData + * @fn ~KinectData(void) + * @brief Default destructor + */ + ~KinectData(void) {} + + /** + * @name IsDepthFrameOk + * @fn bool IsDepthFrameOk(void) const + * @brief Check that the depth frame is NOT empty and that its size + * corresponds to its expected width and height + * @return true if not empty and correct dimensions, false otherwise + */ + bool IsDepthFrameOk(void) const { + return (depth_frame.size() != 0) && + (depth_frame.size() == depth_frame_width * depth_frame_height); + } + + /** + * @name IsRgbFrameOk + * @fn bool IsRgbFrameOk(void) const + * @brief Check that the RGB frame is NOT empty and that its size + * corresponds to its expected width and height + * @return true if not empty and correct dimensions, false otherwise + */ + bool IsRgbFrameOk(void) const { + return (rgb_frame.size() != 0) && + (rgb_frame.size() == rgb_frame_width * rgb_frame_height); + } + }; + + BinSource(const std::string& depth_filename, const std::string& rgb_filename, + bool repeat = false); + + void open(const std::string& depth_filename, const std::string& rgb_filename, + bool repeat = false); + + void release(); + + ~BinSource(); + + bool grab(cv::Mat &depth, cv::Mat &image); + + //parameters taken from camera/oni + + float depth_focal_length_VGA; + float baseline; // mm + double pixelSize; // mm + unsigned short max_depth; // mm + + bool setRegistration (bool value = false); + + private: + + /**< Data struct */ + cv::Ptr kinect_data_; + /**< Filestream of depth maps */ + std::ifstream depth_image_stream_; + /**< Filestram of rgb images */ + std::ifstream rgb_image_stream_; + /**< Current frame's index */ + int cur_frame_; + /**< Total number of frames */ + int total_frames_; + /**< Wether or not to manually align the depth maps and the color images */ + bool manual_align_; + /**< Intrinsics and extrinsics parameters of the Kinect the bin files were recorded with */ + cv::Mat K_ir_; + cv::Mat K_irInv_; + cv::Mat K_rgb_; + cv::Mat C_ir_; + + void getParams (); + +}; +} + +#endif /* __KFUSION_BIN_SOURC__ */ diff --git a/kfusion/src/bin_grabber.cpp b/kfusion/src/bin_grabber.cpp new file mode 100644 index 0000000..85883ab --- /dev/null +++ b/kfusion/src/bin_grabber.cpp @@ -0,0 +1,196 @@ +/** + * @file bin_grabber.hpp + * @brief Bin Grabber for PCL KinFu app + * + * @author Gabriel Cuendet + * @date 21/06/2016 + * Copyright (c) 2016 Gabriel Cuendet. All rights reserved. + */ + +#include +#include +#include +#include +#include +#include + +#include "io/bin_grabber.hpp" + +namespace kfusion { + + BinSource::BinSource(const std::string& depth_filename, const std::string& rgb_filename, + bool repeat) + : manual_align_(false) + , K_ir_((cv::Mat_(3,3) << 589.9, 0.0, 328.4, + 0.0, 589.1, 236.89, + 0.0, 0.0, 1.0)) + , K_rgb_((cv::Mat_(3,3) << 520.9, 0.0, 324.54, + 0.0, 520.2, 237.55, + 0.0, 0.0, 1.0)) + , C_ir_((cv::Mat_(3,1) << -25.4, -0.13, -2.18)) // Ref: http://wiki.ros.org/kinect_calibration/technical + { + cv::invert(K_ir_, K_irInv_); // R_ir is Identity + + kinect_data_ = cv::Ptr(new BinSource::KinectData()); + this->open(depth_filename, rgb_filename, repeat); + } + + void BinSource::open(const std::string& depth_filename, const std::string& rgb_filename, + bool repeat) { + + depth_image_stream_.open(depth_filename.c_str(), std::ios::in | std::ios::binary); + if(!depth_image_stream_.is_open()) { + std::cerr << "[kfusion::BinSource::open] Error: Path is not a regular file: " << depth_filename << std::endl; + } + rgb_image_stream_.open(rgb_filename.c_str(), std::ios::in | std::ios::binary); + if (!rgb_image_stream_.is_open()) { + std::cerr << "[kfusion::BinSource::open] Error: Path is not a regular file: " << rgb_filename << std::endl; + } + + int rgb_num_frames, rgb_block_size; + int rgb_frame_w, rgb_frame_h; + + rgb_image_stream_.read((char*)&rgb_num_frames, sizeof(int)); + rgb_image_stream_.read((char*)&rgb_block_size, sizeof(int)); // size in bytes for 1 frame + rgb_image_stream_.read((char*)&rgb_frame_w, sizeof(int)); + rgb_image_stream_.read((char*)&rgb_frame_h, sizeof(int)); + + int depth_num_frames, depth_block_size; + int depth_frame_w, depth_frame_h; + + depth_image_stream_.read((char*)&depth_num_frames, sizeof(int)); + depth_image_stream_.read((char*)&depth_block_size, sizeof(int)); // size in bytes for 1 frame + depth_image_stream_.read((char*)&depth_frame_w, sizeof(int)); + depth_image_stream_.read((char*)&depth_frame_h, sizeof(int)); + + if (depth_num_frames != rgb_num_frames){ + std::cerr << "[kfusion::BinSource::open] : Watch out not same amount of depth and rgb frames: #depth frames = " << + depth_num_frames << " ; #rgb frames = " << rgb_num_frames << std::endl; + } + + if (depth_num_frames > 0){ + // TODO: Better way to handle different number of frames... + total_frames_ = std::min(depth_num_frames, rgb_num_frames); + + kinect_data_->depth_block_size = depth_block_size; + kinect_data_->depth_frame_width = depth_frame_w; + kinect_data_->depth_frame_height = depth_frame_h; + } else { + std::cerr << "[kfusion::BinSource::open] : no depth frames in the binary file" << std::endl; + } + + if (rgb_num_frames > 0){ + kinect_data_->rgb_block_size = rgb_block_size; + kinect_data_->rgb_frame_width = rgb_frame_w; + kinect_data_->rgb_frame_height = rgb_frame_h; + } else { + std::cerr << "[kfusion::BinSource::open] : no rgb frames in the binary file" << std::endl; + } + } + + void BinSource::release() { + kinect_data_.release(); + } + + BinSource::~BinSource() { + this->release(); + } + + bool BinSource::grab(cv::Mat &depth, cv::Mat &image) { + bool success = false; + + if (cur_frame_ < total_frames_) { + unsigned short crap; + + int depth_frame_size = this->kinect_data_->depth_frame_width * this->kinect_data_->depth_frame_height; + this->kinect_data_->depth_frame.resize(depth_frame_size); + for (int i=0; i < depth_frame_size; ++i) { + depth_image_stream_.read((char*)(&crap), 2); + depth_image_stream_.read((char*)(&(this->kinect_data_->depth_frame[i])), 2); + } + + char alpha; + int rgb_frame_size = this->kinect_data_->rgb_frame_width * this->kinect_data_->rgb_frame_height; + this->kinect_data_->rgb_frame.resize(rgb_frame_size); + for (int i=0; i < rgb_frame_size; ++i) { + rgb_image_stream_.read((char*)(&(this->kinect_data_->rgb_frame[i])), 3); + rgb_image_stream_.read(&alpha, 1); + } + image.create(this->kinect_data_->rgb_frame_height, this->kinect_data_->rgb_frame_width, CV_8UC3); + memcpy(image.data, this->kinect_data_->rgb_frame.data(), 3 * rgb_frame_size); + + success = true; + ++cur_frame_; + + if (manual_align_) { + // Quick check on the data + if (!this->kinect_data_->IsDepthFrameOk()) { + std::cerr << "[BinSource::grab] ERROR: " << + "Depth frame is not loaded properly (size does not correspond or is empty)" << std::endl; + return false; + } + + int depth_size = this->kinect_data_->depth_frame.size(); + int depth_width = this->kinect_data_->depth_frame_width; + + // Allocate space for the new depth frame and initialize at 0 + std::vector aligned_depth_frame(depth_size, 0); + + int d_i; + #pragma omp parallel for + for (d_i = 0; d_i < depth_size; ++d_i) { + float u_ir = d_i % depth_width; + float v_ir = d_i / depth_width; + + cv::Mat uvw_ir = (cv::Mat_(3,1) << u_ir, v_ir, 1.0); + // Backproject to world + cv::Mat xyz = K_irInv_ * uvw_ir; + + // Reproject on RGB camera + cv::Mat uvw_rgb = K_rgb_ * ((static_cast(this->kinect_data_->depth_frame[d_i]) * xyz) - C_ir_); + + // Fill in the new depth frame + int u_rgb = (int)round(uvw_rgb.at(0) / uvw_rgb.at(2)); + int v_rgb = (int)round(uvw_rgb.at(1) / uvw_rgb.at(2)); + if (u_rgb < 0 || u_rgb > depth_width - 1) { + continue; + } + if (v_rgb < 0 || v_rgb > this->kinect_data_->depth_frame_height - 1) { + continue; + } + int j = v_rgb * depth_width + u_rgb; + unsigned short new_depth_val = static_cast(uvw_rgb.at(2)); + + + if (new_depth_val > 0) { + #pragma omp critical + { + // Check that new depth is smaller than current depth (not masked) + if (aligned_depth_frame[j] > 0) { + if (new_depth_val < aligned_depth_frame[j]) + aligned_depth_frame[j] = new_depth_val; + } else { + aligned_depth_frame[j] = new_depth_val; + } + } + } + } + + // Swap the content of depth_frame_ and aligned_depth_frame + this->kinect_data_->depth_frame.swap(aligned_depth_frame); + } + depth.create(this->kinect_data_->depth_frame_height, this->kinect_data_->depth_frame_width, CV_16UC1); + memcpy(depth.data, this->kinect_data_->depth_frame.data(), 2 * depth_frame_size); + } + + return success; + } + + //parameters taken from camera/oni + + bool BinSource::setRegistration (bool value) { + manual_align_ = value; + } + + void BinSource::getParams () {} +} From 6414cff96ec5eeb6d0da9ad6e1ff3c437f3f0969 Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Tue, 28 Jun 2016 17:52:28 +0200 Subject: [PATCH 02/21] Add demo_bin application using BinGrabber --- apps/CMakeLists.txt | 21 +++++- apps/demo_bin.cpp | 158 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 176 insertions(+), 3 deletions(-) create mode 100644 apps/demo_bin.cpp diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index 33731f5..88bc6dd 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -1,7 +1,10 @@ include_directories(${CMAKE_SOURCE_DIR}/kfusion/include) -file(GLOB srcs *.cpp *.hpp) -add_executable(demo ${srcs}) +# Not recommended by CMake to use GLOB +# file(GLOB srcs *.cpp *.hpp) + +# APP 01: OpenNI capture +add_executable(demo demo.cpp) target_link_libraries(demo ${OpenCV_LIBS} kfusion) set_target_properties(demo PROPERTIES @@ -10,4 +13,16 @@ set_target_properties(demo PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin") install(TARGETS demo RUNTIME DESTINATION bin COMPONENT main) -install(FILES ${srcs} DESTINATION app COMPONENT main) +install(FILES demo.cpp DESTINATION app COMPONENT main) + +# APP 02: BinGrabber capture +add_executable(demo_bin demo_bin.cpp) +target_link_libraries(demo_bin ${OpenCV_LIBS} kfusion) + +set_target_properties(demo_bin PROPERTIES + DEBUG_POSTFIX "d" + ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib" + RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin") + +install(TARGETS demo_bin RUNTIME DESTINATION bin COMPONENT main) +install(FILES demo_bin.cpp DESTINATION app COMPONENT main) diff --git a/apps/demo_bin.cpp b/apps/demo_bin.cpp new file mode 100644 index 0000000..8e51f63 --- /dev/null +++ b/apps/demo_bin.cpp @@ -0,0 +1,158 @@ +#include +#include +#include +#include +#include +//#include +#include + +using namespace kfusion; + +struct KinFuApp +{ + static void KeyboardCallback(const cv::viz::KeyboardEvent& event, void* pthis) + { + KinFuApp& kinfu = *static_cast(pthis); + + if(event.action != cv::viz::KeyboardEvent::KEY_DOWN) + return; + + if(event.code == 't' || event.code == 'T') + kinfu.take_cloud(*kinfu.kinfu_); + + if(event.code == 'i' || event.code == 'I') + kinfu.iteractive_mode_ = !kinfu.iteractive_mode_; + } + + KinFuApp(BinSource& source) : exit_ (false), iteractive_mode_(false), capture_ (source), pause_(false) { + KinFuParams params = KinFuParams::default_params(); + kinfu_ = KinFu::Ptr( new KinFu(params) ); + + capture_.setRegistration(true); + + cv::viz::WCube cube(cv::Vec3d::all(0), cv::Vec3d(params.volume_size), true, cv::viz::Color::apricot()); + viz.showWidget("cube", cube, params.volume_pose); + viz.showWidget("coor", cv::viz::WCoordinateSystem(0.1)); + viz.registerKeyboardCallback(KeyboardCallback, this); + } + + void show_depth(const cv::Mat& depth) { + cv::Mat display; + //cv::normalize(depth, display, 0, 255, cv::NORM_MINMAX, CV_8U); + depth.convertTo(display, CV_8U, 255.0/4000); + cv::imshow("Depth", display); + } + + void show_raycasted(KinFu& kinfu) { + const int mode = 3; + if (iteractive_mode_) + kinfu.renderImage(view_device_, viz.getViewerPose(), mode); + else + kinfu.renderImage(view_device_, mode); + + view_host_.create(view_device_.rows(), view_device_.cols(), CV_8UC4); + view_device_.download(view_host_.ptr(), view_host_.step); + cv::imshow("Scene", view_host_); + } + + void take_cloud(KinFu& kinfu) + { + cuda::DeviceArray cloud = kinfu.tsdf().fetchCloud(cloud_buffer); + cv::Mat cloud_host(1, (int)cloud.size(), CV_32FC4); + cloud.download(cloud_host.ptr()); + viz.showWidget("cloud", cv::viz::WCloud(cloud_host)); + //viz.showWidget("cloud", cv::viz::WPaintedCloud(cloud_host)); + } + + bool execute() + { + KinFu& kinfu = *kinfu_; + cv::Mat depth, image; + double time_ms = 0; + bool has_image = false; + + for (int i = 0; !exit_ && !viz.wasStopped(); ++i) + { + bool has_frame = capture_.grab(depth, image); + if (!has_frame) + return std::cout << "Can't grab" << std::endl, false; + + depth_device_.upload(depth.data, depth.step, depth.rows, depth.cols); + + { + SampledScopeTime fps(time_ms); (void)fps; + has_image = kinfu(depth_device_); + } + + if (has_image) + show_raycasted(kinfu); + + show_depth(depth); + //cv::imshow("Image", image); + + if (!iteractive_mode_) + viz.setViewerPose(kinfu.getCameraPose()); + + int key = cv::waitKey(pause_ ? 0 : 3); + + switch(key) + { + case 't': case 'T' : take_cloud(kinfu); break; + case 'i': case 'I' : iteractive_mode_ = !iteractive_mode_; break; + case 27: exit_ = true; break; + case 32: pause_ = !pause_; break; + } + + //exit_ = exit_ || i > 100; + viz.spinOnce(3, true); + } + return true; + } + + bool pause_ /*= false*/; + bool exit_, iteractive_mode_; + BinSource& capture_; + KinFu::Ptr kinfu_; + cv::viz::Viz3d viz; + + cv::Mat view_host_; + cuda::Image view_device_; + cuda::Depth depth_device_; + cuda::DeviceArray cloud_buffer; +}; + + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +int main (int argc, char* argv[]) +{ + int device = 0; + cuda::setDevice (device); + cuda::printShortCudaDeviceInfo (device); + + if(cuda::checkIfPreFermiGPU(device)) + return std::cout << std::endl << "Kinfu is not supported for pre-Fermi GPU architectures, and not built for them by default. Exiting..." << std::endl, 1; + + BinSource capture(argv[1], argv[2]); + + //capture.open("d:/onis/20111013-224932.oni"); + //capture.open("d:/onis/reg20111229-180846.oni"); + //capture.open("d:/onis/white1.oni"); + //capture.open("/media/Main/onis/20111013-224932.oni"); + //capture.open("20111013-225218.oni"); + //capture.open("d:/onis/20111013-224551.oni"); + //capture.open("d:/onis/20111013-224719.oni"); + + KinFuParams my_params = KinFuParams::default_params(); + my_params.volume_dims = Vec3i::all(768); + my_params.volume_size = Vec3i::all(0.4f); + + KinFuApp app (capture); + + // executing + try { app.execute (); } + catch (const std::bad_alloc& /*e*/) { std::cout << "Bad alloc" << std::endl; } + catch (const std::exception& /*e*/) { std::cout << "Exception" << std::endl; } + + return 0; +} From 17bf757f11e8299ad478ed069026b9cbec9014bb Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Wed, 29 Jun 2016 10:03:54 +0200 Subject: [PATCH 03/21] Add a bit of doc, a constructor to initialize kinfu with custom parameters --- apps/demo_bin.cpp | 99 ++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 80 insertions(+), 19 deletions(-) diff --git a/apps/demo_bin.cpp b/apps/demo_bin.cpp index 8e51f63..9581777 100644 --- a/apps/demo_bin.cpp +++ b/apps/demo_bin.cpp @@ -10,6 +10,13 @@ using namespace kfusion; struct KinFuApp { + /** + * @name KeyboardCallback + * @fn static void KeyboardCallback(const cv::viz::KeyboardEvent& event, void* pthis) + * @brief Define keyboard callback for the Kinfu app + * @param[in] event + * @param[in] pthis, void pointer on itself + */ static void KeyboardCallback(const cv::viz::KeyboardEvent& event, void* pthis) { KinFuApp& kinfu = *static_cast(pthis); @@ -21,10 +28,16 @@ struct KinFuApp kinfu.take_cloud(*kinfu.kinfu_); if(event.code == 'i' || event.code == 'I') - kinfu.iteractive_mode_ = !kinfu.iteractive_mode_; + kinfu.interactive_mode_ = !kinfu.interactive_mode_; } - KinFuApp(BinSource& source) : exit_ (false), iteractive_mode_(false), capture_ (source), pause_(false) { + /** + * @name KinFuApp + * @fn KinFuApp(BinSource& source) + * @brief Constructor of the struct, only taking a BinSource data source + * @param[in] source, a BinSource from which the frames are grabbed + */ + KinFuApp(BinSource& source) : exit_ (false), interactive_mode_(false), capture_ (source), pause_(false) { KinFuParams params = KinFuParams::default_params(); kinfu_ = KinFu::Ptr( new KinFu(params) ); @@ -36,6 +49,30 @@ struct KinFuApp viz.registerKeyboardCallback(KeyboardCallback, this); } + /** + * @name KinFuApp + * @fn KinFuApp(BinSource& source, const KinFuParams& params) + * @brief Constructor of the struct, allowing to initialize KinFu to different parameters + * @param[in] source, a BinSource from which the frames are grabbed + * @param[in] params, a KinFuParams struct containing the parameters + */ + KinFuApp(BinSource& source, const KinFuParams& params) : exit_ (false), interactive_mode_(false), capture_ (source), pause_(false) { + kinfu_ = KinFu::Ptr( new KinFu(params) ); + + capture_.setRegistration(true); + + cv::viz::WCube cube(cv::Vec3d::all(0), cv::Vec3d(params.volume_size), true, cv::viz::Color::apricot()); + viz.showWidget("cube", cube, params.volume_pose); + viz.showWidget("coor", cv::viz::WCoordinateSystem(0.1)); + viz.registerKeyboardCallback(KeyboardCallback, this); + } + + /** + * @name show_depth + * @fn void show_depth(const cv::Mat& depth) + * @brief Display the depth stream, after normalization + * @param[in] depth, the depth image to normalize and display + */ void show_depth(const cv::Mat& depth) { cv::Mat display; //cv::normalize(depth, display, 0, 255, cv::NORM_MINMAX, CV_8U); @@ -43,9 +80,15 @@ struct KinFuApp cv::imshow("Depth", display); } + /** + * @name show_raycasted + * @fn void show_raycasted(KinFu& kinfu) + * @brief Show the reconstructed scene (using raycasting) + * @param[in] kinfu instance + */ void show_raycasted(KinFu& kinfu) { const int mode = 3; - if (iteractive_mode_) + if (interactive_mode_) kinfu.renderImage(view_device_, viz.getViewerPose(), mode); else kinfu.renderImage(view_device_, mode); @@ -55,6 +98,12 @@ struct KinFuApp cv::imshow("Scene", view_host_); } + /** + * @name take_cloud + * @fn void take_cloud(KinFu& kinfu) + * @brief Fetch cloud and display it + * @param[in] kinfu instance + */ void take_cloud(KinFu& kinfu) { cuda::DeviceArray cloud = kinfu.tsdf().fetchCloud(cloud_buffer); @@ -64,6 +113,12 @@ struct KinFuApp //viz.showWidget("cloud", cv::viz::WPaintedCloud(cloud_host)); } + /** + * @name execute + * @fn bool execute() + * @brief Run the main loop of the app + * @return true if no error, false otherwise + */ bool execute() { KinFu& kinfu = *kinfu_; @@ -90,7 +145,7 @@ struct KinFuApp show_depth(depth); //cv::imshow("Image", image); - if (!iteractive_mode_) + if (!interactive_mode_) viz.setViewerPose(kinfu.getCameraPose()); int key = cv::waitKey(pause_ ? 0 : 3); @@ -98,7 +153,7 @@ struct KinFuApp switch(key) { case 't': case 'T' : take_cloud(kinfu); break; - case 'i': case 'I' : iteractive_mode_ = !iteractive_mode_; break; + case 'i': case 'I' : interactive_mode_ = !interactive_mode_; break; case 27: exit_ = true; break; case 32: pause_ = !pause_; break; } @@ -109,15 +164,26 @@ struct KinFuApp return true; } - bool pause_ /*= false*/; - bool exit_, iteractive_mode_; + /**< */ + bool pause_; // = false + /**< Stop the execution when set to true */ + bool exit_; + /**< Allow for free point of view (otherwise, follows the camera) */ + bool interactive_mode_; + /**< Reference to a source of data BinSource */ BinSource& capture_; + /**< Pointer to the instance of kinfu */ KinFu::Ptr kinfu_; + /**< */ cv::viz::Viz3d viz; + /**< View of the scene (raycasting) */ cv::Mat view_host_; + /**< View of the scene on the GPU */ cuda::Image view_device_; + /**< Depth frame on the GPU */ cuda::Depth depth_device_; + /**< */ cuda::DeviceArray cloud_buffer; }; @@ -135,19 +201,14 @@ int main (int argc, char* argv[]) BinSource capture(argv[1], argv[2]); - //capture.open("d:/onis/20111013-224932.oni"); - //capture.open("d:/onis/reg20111229-180846.oni"); - //capture.open("d:/onis/white1.oni"); - //capture.open("/media/Main/onis/20111013-224932.oni"); - //capture.open("20111013-225218.oni"); - //capture.open("d:/onis/20111013-224551.oni"); - //capture.open("d:/onis/20111013-224719.oni"); - - KinFuParams my_params = KinFuParams::default_params(); - my_params.volume_dims = Vec3i::all(768); - my_params.volume_size = Vec3i::all(0.4f); + KinFuParams custom_params = KinFuParams::default_params(); + custom_params.volume_dims = Vec3i::all(768); + custom_params.volume_size = Vec3f::all(0.7f); + custom_params.volume_pose = Affine3f().translate(Vec3f(-custom_params.volume_size[0]/2, -custom_params.volume_size[1]/2, 0.5f)); + custom_params.intr = Intr(520.89, 520.23, 324.54, 237.553); + custom_params.tsdf_trunc_dist = 0.005; - KinFuApp app (capture); + KinFuApp app (capture, custom_params); // executing try { app.execute (); } From 1595ac6ede72b0b5220162c6e21e99a2756ccd53 Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Wed, 29 Jun 2016 18:12:57 +0200 Subject: [PATCH 04/21] Add ColorVolume class with partial implementation (cuda kernels are missing) --- kfusion/include/kfusion/cuda/color_volume.hpp | 203 ++++++++++++++++++ kfusion/src/color_volume.cpp | 110 ++++++++++ kfusion/src/cuda/device.hpp | 23 ++ kfusion/src/internal.hpp | 25 +++ kfusion/src/precomp.cpp | 8 +- kfusion/src/precomp.hpp | 1 + 6 files changed, 369 insertions(+), 1 deletion(-) create mode 100644 kfusion/include/kfusion/cuda/color_volume.hpp create mode 100644 kfusion/src/color_volume.cpp diff --git a/kfusion/include/kfusion/cuda/color_volume.hpp b/kfusion/include/kfusion/cuda/color_volume.hpp new file mode 100644 index 0000000..dfbd42e --- /dev/null +++ b/kfusion/include/kfusion/cuda/color_volume.hpp @@ -0,0 +1,203 @@ +// +// Created by gabriel on 29.06.16. +// + +#ifndef KFUSION_COLOR_VOLUME_HPP +#define KFUSION_COLOR_VOLUME_HPP + +#include + +namespace kfusion +{ + namespace cuda + { + /** + * @class ColorVolume + * @brief Color volume class + */ + class KF_EXPORTS ColorVolume + { + public: + +#pragma mark - +#pragma mark Initialization + + /** + * @name ColorVolume + * @fn ColorVolume (const cv::Vec3i &dims) + * @brief Constructor of the class + * @param[in] Number of voxels for each dimensions + */ + ColorVolume (const cv::Vec3i &dims); + + /** + * @name ~ColorVolume + * @fn virtual ~ColorVolume(void) + * @brief Destructor of the class + */ + virtual ~ColorVolume (void); + + /** + * @name create + * @fn void create(const Vec3i& dims) + * @brief Initialize the volume on the device + * @param[in] Number of voxels for each dimensions + */ + void create(const Vec3i& dims); + +#pragma mark - +#pragma mark Getters and Setters + + /** + * @name getDims + * @fn Vec3i getDims() const + * @brief Getter for the dimensions + * @return Number of voxels along each dimensions + */ + Vec3i getDims() const {return dims_;} + + /** + * @name getVoxelSize + * @fn Vec3f getVoxelSize() const + * @brief Size of a voxel, given by each dimension's size divided by + * the number of voxel along that dimension + * @return Vector of voxel size along each dimension + */ + Vec3f getVoxelSize() const {return Vec3f(size_[0]/dims_[0], size_[1]/dims_[1], size_[2]/dims_[2]);} + + /** + * @name data + * @fn const CudaData data() const + * @brief Const getter for the data + */ + const CudaData data() const {return data_;} + + /** + * @name data + * @fn const CudaData data() const + * @brief Non-const getter for the data + */ + CudaData data() {return data_;} + + /** + * @name getSize + * @fn Vec3f getSize() const + * @brief Getter for volume size + */ + Vec3f getSize() const {return size_;} + + /** + * @name setSize + * @fn void setSize(const Vec3f& size) + * @brief Setter for volume size + */ + void setSize(const Vec3f& size) {size_ = size; setTruncDist(trunc_dist_);} + + /** + * @name getTruncDist + * @fn float getTruncDist() const + * @brief Getter for the TruncDist + */ + float getTruncDist() const {return trunc_dist_;} + + /** + * @name setTruncDist + * @fn void setTruncDist(float distance) + * @brief Setter for the TruncDist + */ + void setTruncDist(float distance); + + /** + * @name getMaxWeight + * @fn int getMaxWeight() const + * @brief Getter for the MaxWeight + */ + int getMaxWeight() const {return max_weight_;} + + /** + * @name setMaxWeight + * @fn void getMaxWeight(int weight) + * @brief Setter for the MaxWeight + */ + void setMaxWeight(int weight) {max_weight_ = weight;} + + /** + * @name getPose + * @fn Affine3f getPose() const + * @brief Getter for the pose + */ + Affine3f getPose() const {return pose_;} + + /** + * @name setPose + * @fn void setPose(const Affine3f& pose) + * @brief Setter for the pose + */ + void setPose(const Affine3f& pose) {pose_ = pose;} + +#pragma mark - +#pragma mark Usage + + /** + * @name clear + * @fn virtual void clear() + * @brief Allocate memory on device and initialize at 0 + */ + virtual void clear(); + + /** + * @name applyAffine + * @fn virtual void applyAffine(const Affine3f& affine) + * @brief Apply an affine transformation on pose + * @param[in] affine, the transformation to apply + */ + virtual void applyAffine(const Affine3f& affine) {pose_ = affine * pose_;} + + /** + * @name integrate + * @fn virtual void integrate(const Image& rgb_image, const Affine3f& camera_pose, const Intr& intr) + * @brief + * @param[in] rgb_image, the new frame to integrate + * @param[in] camera_pose, the current pose of the camera + * @param[in] intr, the intrinsic parameters of the RGB camera + */ + virtual void integrate(const Image& rgb_image, const Affine3f& camera_pose, const Intr& intr); + + /** + * @name swap + * @fn void swap(CudaData& data) + * @brief Swap memory content + */ + void swap(CudaData& data) {data_.swap(data);} + + /** + * @name fetchColors + * @fn void fetchColors(const DeviceArray& cloud, DeviceArray& colors) const + * @brief Gets color for a point cloud + * @param[in] cloud, the coordinates of the colors to extract + * @param[in] colors, the colors stored in the volume + */ + void fetchColors(const DeviceArray& cloud, DeviceArray& colors) const; + + private: + +#pragma mark - +#pragma mark Private attributes + + /**< Memory on the device */ + CudaData data_; + /**< The truncation distance of the volume */ + float trunc_dist_; + /**< Maximum weight */ + int max_weight_; + /**< Number of voxels along each coordinates */ + Vec3i dims_; + /**< Size of the volume along each coordinates */ + Vec3f size_; + /**< The pose of the volume in the world */ + Affine3f pose_; + }; + } +} + +#endif //KFUSION_COLOR_VOLUME_HPP diff --git a/kfusion/src/color_volume.cpp b/kfusion/src/color_volume.cpp new file mode 100644 index 0000000..f3908ee --- /dev/null +++ b/kfusion/src/color_volume.cpp @@ -0,0 +1,110 @@ +// +// Created by gabriel on 29.06.16. +// + +#include "precomp.hpp" + +//////////////////////////////////////////////////////////////////////////////// +// TsdfVolume + +#pragma mark - +#pragma mark Initialization + +/* + * @name ColorVolume + * @fn ColorVolume (const cv::Vec3i &dims) + * @brief Constructor of the class + * @param[in] Number of voxels for each dimensions + */ +kfusion::cuda::ColorVolume::ColorVolume(const Vec3i& dims) + : data_(), trunc_dist_(0.03f), max_weight_(128), dims_(dims), + size_(Vec3f::all(3.f)), pose_(Affine3f::Identity()) +{ + create(dims_); +} + +/* + * @name ~ColorVolume + * @fn virtual ~ColorVolume(void) + * @brief Destructor of the class + */ +kfusion::cuda::ColorVolume::~ColorVolume() {} + +/* + * @name create + * @fn void create(const Vec3i& dims) + * @brief Initialize the volume on the device + * @param[in] Number of voxels for each dimensions + */ +void kfusion::cuda::ColorVolume::create(const Vec3i& dims) +{ + dims_ = dims; + int voxels_number = dims_[0] * dims_[1] * dims_[2]; + data_.create(voxels_number * 4 * sizeof(unsigned char)); + setTruncDist(trunc_dist_); + clear(); +} + +#pragma mark - +#pragma mark Getters and Setters + +void kfusion::cuda::ColorVolume::setTruncDist(float distance) +{ + Vec3f vsz = getVoxelSize(); + float max_coeff = std::max(std::max(vsz[0], vsz[1]), vsz[2]); + trunc_dist_ = std::max (distance, 2.1f * max_coeff); +} + +#pragma mark - +#pragma mark Usage + +/* + * @name clear + * @fn virtual void clear() + * @brief Allocate memory on device and initialize at 0 + */ +void kfusion::cuda::ColorVolume::clear() +{ + device::Vec3i dims = device_cast(dims_); + device::Vec3f vsz = device_cast(getVoxelSize()); + + device::ColorVolume volume(data_.ptr(), dims, vsz, trunc_dist_, max_weight_); + device::clear_volume(volume); +} + +/* + * @name integrate + * @fn virtual void integrate(const Image& rgb_image, const Affine3f& camera_pose, const Intr& intr) + * @brief + * @param[in] rgb_image, the new frame to integrate + * @param[in] camera_pose, the current pose of the camera + * @param[in] intr, the intrinsic parameters of the RGB camera + */ +void kfusion::cuda::ColorVolume::integrate(const Image& rgb_image, + const Affine3f& camera_pose, + const Intr& intr) +{ + Affine3f vol2cam = camera_pose.inv() * pose_; + + device::Projector proj(intr.fx, intr.fy, intr.cx, intr.cy); + + device::Vec3i dims = device_cast(dims_); + device::Vec3f vsz = device_cast(getVoxelSize()); + device::Aff3f aff = device_cast(vol2cam); + + device::ColorVolume volume(data_.ptr(), dims, vsz, trunc_dist_, max_weight_); + device::integrate(rgb_image, volume, aff, proj); +} + +/* + * @name fetchColors + * @fn void fetchColors(const DeviceArray& cloud, DeviceArray& colors) const + * @brief Gets color for a point cloud + * @param[in] cloud, the coordinates of the colors to extract + * @param[in] colors, the colors stored in the volume + */ +void kfusion::cuda::ColorVolume::fetchColors(const DeviceArray& cloud, + DeviceArray& colors) const +{ + // @TODO: (Gabriel) Implement! (with corresponding CUDA kernel in color_volume.cu) +} \ No newline at end of file diff --git a/kfusion/src/cuda/device.hpp b/kfusion/src/cuda/device.hpp index 497c8f8..5a252be 100644 --- a/kfusion/src/cuda/device.hpp +++ b/kfusion/src/cuda/device.hpp @@ -26,6 +26,29 @@ __kf_device__ kfusion::device::TsdfVolume::elem_type* kfusion::device::TsdfVolum __kf_device__ kfusion::device::TsdfVolume::elem_type* kfusion::device::TsdfVolume::zstep(elem_type *const ptr) const { return ptr + dims.x * dims.y; } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// ColorVolume + +//__kf_device__ +//kfusion::device::TsdfVolume::TsdfVolume(elem_type* _data, int3 _dims, float3 _voxel_size, float _trunc_dist, int _max_weight) +// : data(_data), dims(_dims), voxel_size(_voxel_size), trunc_dist(_trunc_dist), max_weight(_max_weight) {} + +//__kf_device__ +//kfusion::device::TsdfVolume::TsdfVolume(const TsdfVolume& other) +// : data(other.data), dims(other.dims), voxel_size(other.voxel_size), trunc_dist(other.trunc_dist), max_weight(other.max_weight) {} + +__kf_device__ kfusion::device::ColorVolume::elem_type* kfusion::device::ColorVolume::operator()(int x, int y, int z) +{ return data + x + y*dims.x + z*dims.y*dims.x; } + +__kf_device__ const kfusion::device::ColorVolume::elem_type* kfusion::device::ColorVolume::operator() (int x, int y, int z) const +{ return data + x + y*dims.x + z*dims.y*dims.x; } + +__kf_device__ kfusion::device::ColorVolume::elem_type* kfusion::device::ColorVolume::beg(int x, int y) const +{ return data + x + dims.x * y; } + +__kf_device__ kfusion::device::ColorVolume::elem_type* kfusion::device::ColorVolume::zstep(elem_type *const ptr) const +{ return ptr + dims.x * dims.y; } + //////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// Projector diff --git a/kfusion/src/internal.hpp b/kfusion/src/internal.hpp index 3a264be..10c77e2 100644 --- a/kfusion/src/internal.hpp +++ b/kfusion/src/internal.hpp @@ -48,6 +48,28 @@ namespace kfusion TsdfVolume& operator=(const TsdfVolume&); }; + struct ColorVolume + { + public: + typedef uchar4 elem_type; + + elem_type *const data; + const int3 dims; + const float3 voxel_size; + const float trunc_dist; + const int max_weight; + + ColorVolume(elem_type* data, int3 dims, float3 voxel_size, float trunc_dist, int max_weight); + //TsdfVolume(const TsdfVolume&); + + __kf_device__ elem_type* operator()(int x, int y, int z); + __kf_device__ const elem_type* operator() (int x, int y, int z) const ; + __kf_device__ elem_type* beg(int x, int y) const; + __kf_device__ elem_type* zstep(elem_type *const ptr) const; + private: + ColorVolume& operator=(const ColorVolume&); + }; + struct Projector { float2 f, c; @@ -115,6 +137,9 @@ namespace kfusion __kf_device__ float unpack_tsdf(ushort2 value, int& weight); __kf_device__ float unpack_tsdf(ushort2 value); + //color volume functions + void clear_volume(ColorVolume volume); + void integrate(const Image& image, ColorVolume& volume, const Aff3f& aff, const Projector& proj); //image proc functions void compute_dists(const Depth& depth, Dists dists, float2 f, float2 c); diff --git a/kfusion/src/precomp.cpp b/kfusion/src/precomp.cpp index 080169b..60fd002 100644 --- a/kfusion/src/precomp.cpp +++ b/kfusion/src/precomp.cpp @@ -6,7 +6,7 @@ kfusion::Intr::Intr () {} kfusion::Intr::Intr (float fx_, float fy_, float cx_, float cy_) : fx(fx_), fy(fy_), cx(cx_), cy(cy_) {} - + kfusion::Intr kfusion::Intr::operator()(int level_index) const { int div = 1 << level_index; @@ -36,6 +36,12 @@ kfusion::device::TsdfVolume::TsdfVolume(elem_type* _data, int3 _dims, float3 _vo //kfusion::device::TsdfVolume::elem_type* kfusionl::device::TsdfVolume::zstep(elem_type *const ptr) const //{ return data + dims.x * dims.y; } +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// ColorVolume host implementation + +kfusion::device::ColorVolume::ColorVolume(elem_type* _data, int3 _dims, float3 _voxel_size, float _trunc_dist, int _max_weight) + : data(_data), dims(_dims), voxel_size(_voxel_size), trunc_dist(_trunc_dist), max_weight(_max_weight) {} + //////////////////////////////////////////////////////////////////////////////////////////////////////////////// /// Projector host implementation diff --git a/kfusion/src/precomp.hpp b/kfusion/src/precomp.hpp index 52b054c..5940851 100644 --- a/kfusion/src/precomp.hpp +++ b/kfusion/src/precomp.hpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include //#include From dcfaa7567b2552efca4666a412fa052cedd80021 Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Thu, 30 Jun 2016 18:07:55 +0200 Subject: [PATCH 05/21] Add first version of the ColorVolume cuda kernel --- kfusion/include/kfusion/cuda/color_volume.hpp | 3 +- kfusion/src/color_volume.cpp | 4 +- kfusion/src/cuda/color_volume.cu | 190 ++++++++++++++++++ kfusion/src/internal.hpp | 4 +- 4 files changed, 198 insertions(+), 3 deletions(-) create mode 100644 kfusion/src/cuda/color_volume.cu diff --git a/kfusion/include/kfusion/cuda/color_volume.hpp b/kfusion/include/kfusion/cuda/color_volume.hpp index dfbd42e..6279ee6 100644 --- a/kfusion/include/kfusion/cuda/color_volume.hpp +++ b/kfusion/include/kfusion/cuda/color_volume.hpp @@ -158,10 +158,11 @@ namespace kfusion * @fn virtual void integrate(const Image& rgb_image, const Affine3f& camera_pose, const Intr& intr) * @brief * @param[in] rgb_image, the new frame to integrate + * @param[in] depth_map, the raycasted depth map * @param[in] camera_pose, the current pose of the camera * @param[in] intr, the intrinsic parameters of the RGB camera */ - virtual void integrate(const Image& rgb_image, const Affine3f& camera_pose, const Intr& intr); + virtual void integrate(const Image& rgb_image, const Dists& depth_map, const Affine3f& camera_pose, const Intr& intr); /** * @name swap diff --git a/kfusion/src/color_volume.cpp b/kfusion/src/color_volume.cpp index f3908ee..8a7dc76 100644 --- a/kfusion/src/color_volume.cpp +++ b/kfusion/src/color_volume.cpp @@ -77,10 +77,12 @@ void kfusion::cuda::ColorVolume::clear() * @fn virtual void integrate(const Image& rgb_image, const Affine3f& camera_pose, const Intr& intr) * @brief * @param[in] rgb_image, the new frame to integrate + * @param[in] depth_map, the raycasted depth map * @param[in] camera_pose, the current pose of the camera * @param[in] intr, the intrinsic parameters of the RGB camera */ void kfusion::cuda::ColorVolume::integrate(const Image& rgb_image, + const Dists& depth_map, const Affine3f& camera_pose, const Intr& intr) { @@ -93,7 +95,7 @@ void kfusion::cuda::ColorVolume::integrate(const Image& rgb_image, device::Aff3f aff = device_cast(vol2cam); device::ColorVolume volume(data_.ptr(), dims, vsz, trunc_dist_, max_weight_); - device::integrate(rgb_image, volume, aff, proj); + device::integrate(rgb_image, depth_map, volume, aff, proj); } /* diff --git a/kfusion/src/cuda/color_volume.cu b/kfusion/src/cuda/color_volume.cu new file mode 100644 index 0000000..9c4d2b2 --- /dev/null +++ b/kfusion/src/cuda/color_volume.cu @@ -0,0 +1,190 @@ +#include "device.hpp" +#include "texture_binder.hpp" +#include "../internal.hpp" + +using namespace kfusion::device; + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// Volume initialization + +namespace kfusion +{ + namespace device + { + __global__ void clear_volume_kernel(ColorVolume color) + { + int x = threadIdx.x + blockIdx.x * blockDim.x; + int y = threadIdx.y + blockIdx.y * blockDim.y; + + if (x < color.dims.x && y < color.dims.y) + { + uchar4 *beg = color.beg(x, y); + uchar4 *end = beg + color.dims.x * color.dims.y * color.dims.z; + + for(uchar4* pos = beg; pos != end; pos = color.zstep(pos)) + *pos = make_uchar4 (0, 0, 0, 0); + } + } + } +} + +void kfusion::device::clear_volume(ColorVolume volume) +{ + dim3 block (32, 8); + dim3 grid (1, 1, 1); + grid.x = divUp (volume.dims.x, block.x); + grid.y = divUp (volume.dims.y, block.y); + + clear_volume_kernel<<>>(volume); + cudaSafeCall ( cudaGetLastError () ); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// Volume integration + +namespace kfusion +{ + namespace device + { + texture image_tex(0, cudaFilterModePoint, cudaAddressModeBorder, cudaCreateChannelDescHalf()); + texture depth_tex(0, cudaFilterModePoint, cudaAddressModeBorder, cudaCreateChannelDescHalf()); + + struct ColorIntegrator { + Aff3f vol2cam; + PtrStep vmap; + Projector proj; + int2 im_size; + + float tranc_dist_inv; + + __kf_device__ + void operator()(ColorVolume& volume) const + { + int x = blockIdx.x * blockDim.x + threadIdx.x; + int y = blockIdx.y * blockDim.y + threadIdx.y; + + if (x >= volume.dims.x || y >= volume.dims.y) + return; + + float3 zstep = make_float3(vol2cam.R.data[0].z, vol2cam.R.data[1].z, vol2cam.R.data[2].z) * volume.voxel_size.z; + + float3 vx = make_float3(x * volume.voxel_size.x, y * volume.voxel_size.y, 0); + float3 vc = vol2cam * vx; //tranform from volume coo frame to camera one + + ColorVolume::elem_type* vptr = volume.beg(x, y); + for(int i = 0; i < volume.dims.z; ++i, vc += zstep, vptr = volume.zstep(vptr)) + { + float2 coo = proj(vc); // project to image coordinate + // check wether coo in inside the image boundaries + if (coo.x >= 0.0 && coo.y >= 0.0 && + coo.x < im_size.x && coo.y < im_size.y) { + + float Dp = tex2D(depth_tex, coo.x, coo.y); + if(Dp == 0 || vc.z <= 0) + continue; + + bool update = false; + // Check the distance + float sdf = Dp - __fsqrt_rn(dot(vc, vc)); //Dp - norm(v) + update = sdf > -volume.trunc_dist && sdf < volume.trunc_dist; + if (update) + { + // Read the existing value and weight + uchar4 volume_rgbw = *vptr; + int weight_prev = volume_rgbw.w; + + // Average with new value and weight + uchar4 rgb = tex2D(image_tex, coo.x, coo.y); + + const float Wrk = 1.f; + float new_x = (volume_rgbw.x * weight_prev + Wrk * rgb.x) / (weight_prev + Wrk); + float new_y = (volume_rgbw.y * weight_prev + Wrk * rgb.y) / (weight_prev + Wrk); + float new_z = (volume_rgbw.z * weight_prev + Wrk * rgb.z) / (weight_prev + Wrk); + + int weight_new = weight_prev + 1; + + uchar4 volume_rgbw_new; + volume_rgbw_new.x = min (255, max (0, __float2int_rn (new_x))); + volume_rgbw_new.y = min (255, max (0, __float2int_rn (new_y))); + volume_rgbw_new.z = min (255, max (0, __float2int_rn (new_z))); + volume_rgbw_new.w = min (volume.max_weight, weight_new); + + // Write back + *vptr = volume_rgbw_new; + } + } // in camera image range + } // for (int i=0; i& rgb_image, + const PtrStepSz& depth_map, + ColorVolume& volume, + const Aff3f& aff, + const Projector& proj) +{ + ColorIntegrator ti; + ti.im_size = make_int2(rgb_image.cols, rgb_image.rows); + ti.vol2cam = aff; + ti.proj = proj; + ti.tranc_dist_inv = 1.f/volume.trunc_dist; + + image_tex.filterMode = cudaFilterModePoint; + image_tex.addressMode[0] = cudaAddressModeBorder; + image_tex.addressMode[1] = cudaAddressModeBorder; + image_tex.addressMode[2] = cudaAddressModeBorder; + TextureBinder image_binder(rgb_image, image_tex, cudaCreateChannelDescHalf()); (void)image_binder; + + depth_tex.filterMode = cudaFilterModePoint; + depth_tex.addressMode[0] = cudaAddressModeBorder; + depth_tex.addressMode[1] = cudaAddressModeBorder; + depth_tex.addressMode[2] = cudaAddressModeBorder; + TextureBinder depth_binder(depth_map, depth_tex, cudaCreateChannelDescHalf()); (void)depth_binder; + + dim3 block(32, 8); + dim3 grid(divUp(volume.dims.x, block.x), divUp(volume.dims.y, block.y)); + + integrate_kernel<<>>(ti, volume); + cudaSafeCall ( cudaGetLastError () ); + cudaSafeCall ( cudaDeviceSynchronize() ); +} + +namespace kfusion +{ + namespace device + { + __global__ void + fetchColors_kernel (const float3 cell_size, const ColorVolume &volume, + const PtrSz &points, PtrSz &colors) + { + int idx = blockIdx.x * blockDim.x + threadIdx.x; + + if (idx < points.size) + { + int3 v; + float3 p = *(const float3 *) (points.data + idx); + v.x = __float2int_rd( + p.x / cell_size.x); // round to negative infinity + v.y = __float2int_rd(p.y / cell_size.y); + v.z = __float2int_rd(p.z / cell_size.z); + + uchar4 rgbw = *volume(v.x, v.y, v.z); + colors[idx] = make_uchar4(rgbw.z, rgbw.y, rgbw.x, 0); //bgra + } + } + } +} + +void +kfusion::device::fetchColors(const ColorVolume& volume, const PtrSz& points, PtrSz& colors) +{ + const int block = 256; + float3 cell_size = make_float3 (volume.voxel_size.x, volume.voxel_size.y, volume.voxel_size.z); + fetchColors_kernel<<>>(cell_size, volume, points, colors); + cudaSafeCall ( cudaGetLastError () ); + cudaSafeCall (cudaDeviceSynchronize ()); +}; \ No newline at end of file diff --git a/kfusion/src/internal.hpp b/kfusion/src/internal.hpp index 10c77e2..abf4020 100644 --- a/kfusion/src/internal.hpp +++ b/kfusion/src/internal.hpp @@ -19,6 +19,7 @@ namespace kfusion typedef DeviceArray2D Depth; typedef DeviceArray2D Normals; typedef DeviceArray2D Points; + typedef PtrStepSz Colors; // Not sure it's needed typedef DeviceArray2D Image; typedef int3 Vec3i; @@ -139,7 +140,8 @@ namespace kfusion //color volume functions void clear_volume(ColorVolume volume); - void integrate(const Image& image, ColorVolume& volume, const Aff3f& aff, const Projector& proj); + void integrate(const Colors& image, const Dists& depth_map, ColorVolume& volume, const Aff3f& aff, const Projector& proj); + void fetchColors(const ColorVolume& volume, const PtrSz& points, PtrSz& colors); //image proc functions void compute_dists(const Depth& depth, Dists dists, float2 f, float2 c); From a1cb5247e27e0c6999ba8c0e14dbb377e4f45b39 Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Thu, 30 Jun 2016 18:10:04 +0200 Subject: [PATCH 06/21] Remove unnecessary methods from TsdfVolume --- kfusion/include/kfusion/cuda/tsdf_volume.hpp | 4 ++-- kfusion/src/tsdf_volume.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/kfusion/include/kfusion/cuda/tsdf_volume.hpp b/kfusion/include/kfusion/cuda/tsdf_volume.hpp index b38768b..b72b052 100644 --- a/kfusion/include/kfusion/cuda/tsdf_volume.hpp +++ b/kfusion/include/kfusion/cuda/tsdf_volume.hpp @@ -38,8 +38,8 @@ namespace kfusion float getGradientDeltaFactor() const; void setGradientDeltaFactor(float factor); - Vec3i getGridOrigin() const; - void setGridOrigin(const Vec3i& origin); + //Vec3i getGridOrigin() const; + //void setGridOrigin(const Vec3i& origin); virtual void clear(); virtual void applyAffine(const Affine3f& affine); diff --git a/kfusion/src/tsdf_volume.cpp b/kfusion/src/tsdf_volume.cpp index a26a88c..3a76ba6 100644 --- a/kfusion/src/tsdf_volume.cpp +++ b/kfusion/src/tsdf_volume.cpp @@ -25,7 +25,7 @@ void kfusion::cuda::TsdfVolume::create(const Vec3i& dims) { dims_ = dims; int voxels_number = dims_[0] * dims_[1] * dims_[2]; - data_.create(voxels_number * sizeof(int)); + data_.create(voxels_number * 2 * sizeof(unsigned short)); setTruncDist(trunc_dist_); clear(); } @@ -66,7 +66,7 @@ void kfusion::cuda::TsdfVolume::swap(CudaData& data) { data_.swap(data); } void kfusion::cuda::TsdfVolume::applyAffine(const Affine3f& affine) { pose_ = affine * pose_; } void kfusion::cuda::TsdfVolume::clear() -{ +{ device::Vec3i dims = device_cast(dims_); device::Vec3f vsz = device_cast(getVoxelSize()); From 35faa4f0491b322160cbe43e48682cd7ed85b626 Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Fri, 1 Jul 2016 10:28:00 +0200 Subject: [PATCH 07/21] Small changes in kinfu --- kfusion/include/kfusion/kinfu.hpp | 29 +++++++++++++++++------------ 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/kfusion/include/kfusion/kinfu.hpp b/kfusion/include/kfusion/kinfu.hpp index 5fd7836..ce89045 100644 --- a/kfusion/include/kfusion/kinfu.hpp +++ b/kfusion/include/kfusion/kinfu.hpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -25,24 +26,27 @@ namespace kfusion int cols; //pixels int rows; //pixels + bool integrate_color; //Color integration Intr intr; //Camera parameters Vec3i volume_dims; //number of voxels Vec3f volume_size; //meters Affine3f volume_pose; //meters, inital pose - float bilateral_sigma_depth; //meters - float bilateral_sigma_spatial; //pixels - int bilateral_kernel_size; //pixels + float bilateral_sigma_depth; //meters + float bilateral_sigma_spatial; //pixels + int bilateral_kernel_size; //pixels - float icp_truncate_depth_dist; //meters - float icp_dist_thres; //meters - float icp_angle_thres; //radians - std::vector icp_iter_num; //iterations for level index 0,1,..,3 + float icp_truncate_depth_dist; //meters + float icp_dist_thres; //meters + float icp_angle_thres; //radians + std::vector icp_iter_num; //iterations for level index 0,1,..,3 float tsdf_min_camera_movement; //meters, integrate only if exceedes - float tsdf_trunc_dist; //meters; - int tsdf_max_weight; //frames + float tsdf_trunc_dist; //meters; + int tsdf_max_weight; //frames + + int color_max_weight; //frames float raycast_step_factor; // in voxel sizes float gradient_delta_factor; // in voxel sizes @@ -53,7 +57,7 @@ namespace kfusion class KF_EXPORTS KinFu { - public: + public: typedef cv::Ptr Ptr; KinFu(const KinFuParams& params); @@ -69,7 +73,7 @@ namespace kfusion void reset(); - bool operator()(const cuda::Depth& dpeth, const cuda::Image& image = cuda::Image()); + bool operator()(const cuda::Depth& depth, const cuda::Image& image = cuda::Image()); void renderImage(cuda::Image& image, int flags = 0); void renderImage(cuda::Image& image, const Affine3f& pose, int flags = 0); @@ -90,7 +94,8 @@ namespace kfusion cuda::Normals normals_; cuda::Depth depths_; - cv::Ptr volume_; + cv::Ptr tsdf_volume_; + cv::Ptr color_volume_; cv::Ptr icp_; }; } From 22218a5a4bcd37332d3071fe5419871c73dab309 Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Mon, 4 Jul 2016 11:08:58 +0200 Subject: [PATCH 08/21] Add possibility to compile without OpenNI, automatically disabling capture warper --- CMakeLists.txt | 2 +- kfusion/CMakeLists.txt | 94 +++++++++++++++++++++++++++++++++++++++++- 2 files changed, 93 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 80b5886..d2e5081 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,7 +39,7 @@ endif() # ---[ find dependencies find_package(OpenCV 2.4.8 REQUIRED COMPONENTS core viz highgui) find_package(CUDA 5.0 REQUIRED) -find_package(OpenNI REQUIRED) +find_package(OpenNI) include_directories(${OpenCV_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS} ${OPENNI_INCLUDE_DIR}) # ---[ misc settings diff --git a/kfusion/CMakeLists.txt b/kfusion/CMakeLists.txt index e0cdc73..441f7dc 100644 --- a/kfusion/CMakeLists.txt +++ b/kfusion/CMakeLists.txt @@ -1,3 +1,93 @@ set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} "--ftz=true;--prec-div=false;--prec-sqrt=false") -add_module_library(kfusion) -target_link_libraries(kfusion ${CUDA_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${OpenCV_LIBS} ${OPENNI_LIBRARY}) + +#add_module_library(kfusion) +set(module_name kfusion) +set(Includes_srcs + include/io/bin_grabber.hpp + include/${module_name}/exports.hpp + include/${module_name}/kinfu.hpp + include/${module_name}/types.hpp) + +set(Includes_cuda_srcs + include/${module_name}/cuda/color_volume.hpp + include/${module_name}/cuda/device_array.hpp + include/${module_name}/cuda/device_memory.hpp + include/${module_name}/cuda/imgproc.hpp + include/${module_name}/cuda/kernel_containers.hpp + include/${module_name}/cuda/projective_icp.hpp + include/${module_name}/cuda/tsdf_volume.hpp) + +set(Source_srcs + src/bin_grabber.cpp + src/color_volume.cpp + src/core.cpp + src/device_memory.cpp + src/imgproc.cpp + src/internal.hpp + src/kinfu.cpp + src/precomp.cpp + src/precomp.hpp + src/projective_icp.cpp + src/safe_call.hpp + src/tsdf_volume.cpp) + +set(Source_cuda_srcs + src/cuda/color_volume.cu + src/cuda/device.hpp + src/cuda/imgproc.cu + src/cuda/proj_icp.cu + src/cuda/temp_utils.hpp + src/cuda/texture_binder.hpp + src/cuda/tsdf_volume.cu) + +if(OPENNI_FOUND) + set(Source_srcs ${Source_srcs} + src/capture.cpp) + set(Include_srcs ${Include_srcs} + include/io/capture.hpp) +endif(OPENNI_FOUND) + +include_directories(include src src/cuda) + +set(__has_cuda OFF) +check_cuda(__has_cuda) + +set(__lib_type STATIC) +if (${ARGV1} MATCHES "SHARED|STATIC") + set(__lib_type ${ARGV1}) +endif() + +if (__has_cuda) + cuda_add_library(${module_name} ${__lib_type} ${Source_srcs} ${Source_cuda_srcs} ) +else() + add_library(${module_name} ${__lib_type} ${Source_srcs}) +endif() + +if(MSVC) + set_target_properties(${module_name} PROPERTIES DEFINE_SYMBOL KFUSION_API_EXPORTS) +else() + add_definitions(-DKFUSION_API_EXPORTS) +endif() + +default_properties(${module_name}) + +if(USE_PROJECT_FOLDERS) + set_target_properties(${module_name} PROPERTIES FOLDER "Libraries") +endif() + +set_target_properties(${module_name} PROPERTIES INSTALL_NAME_DIR lib) + +install(TARGETS ${module_name} + RUNTIME DESTINATION bin COMPONENT main + LIBRARY DESTINATION lib COMPONENT main + ARCHIVE DESTINATION lib COMPONENT main) + +install(DIRECTORY include/ DESTINATION include/ FILES_MATCHING PATTERN "*.h*") + +target_link_libraries(kfusion ${CUDA_LIBRARIES} ${CUDA_CUDA_LIBRARY} ${OpenCV_LIBS}) + +MESSAGE("OpenNI FOUND = ${OPENNI_FOUND}") + +if (OPENNI_FOUND) + target_link_libraries(kfusion ${OPENNI_LIBRARY}) +endif(OPENNI_FOUND) \ No newline at end of file From b73dcd2c8f84cf266026f0547fa299f50ad99874 Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Mon, 4 Jul 2016 11:10:44 +0200 Subject: [PATCH 09/21] Add gitignore file --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..5f73a11 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +.idea/ +build/ + From b839e45d02bf6a1f8d241ecb131b09bfa08ac2b9 Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Mon, 4 Jul 2016 11:31:29 +0200 Subject: [PATCH 10/21] Add color_volume to app --- apps/CMakeLists.txt | 18 +++++---- apps/demo_bin.cpp | 24 +++++++++--- kfusion/include/kfusion/kinfu.hpp | 3 ++ kfusion/src/kinfu.cpp | 61 +++++++++++++++++++++++-------- 4 files changed, 78 insertions(+), 28 deletions(-) diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index 88bc6dd..d26866e 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -4,16 +4,18 @@ include_directories(${CMAKE_SOURCE_DIR}/kfusion/include) # file(GLOB srcs *.cpp *.hpp) # APP 01: OpenNI capture -add_executable(demo demo.cpp) -target_link_libraries(demo ${OpenCV_LIBS} kfusion) +if (OPENNI_FOUND) + add_executable(demo demo.cpp) + target_link_libraries(demo ${OpenCV_LIBS} kfusion) -set_target_properties(demo PROPERTIES - DEBUG_POSTFIX "d" - ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib" - RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin") + set_target_properties(demo PROPERTIES + DEBUG_POSTFIX "d" + ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/lib" + RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin") -install(TARGETS demo RUNTIME DESTINATION bin COMPONENT main) -install(FILES demo.cpp DESTINATION app COMPONENT main) + install(TARGETS demo RUNTIME DESTINATION bin COMPONENT main) + install(FILES demo.cpp DESTINATION app COMPONENT main) +endif(OPENNI_FOUND) # APP 02: BinGrabber capture add_executable(demo_bin demo_bin.cpp) diff --git a/apps/demo_bin.cpp b/apps/demo_bin.cpp index 9581777..c2902e4 100644 --- a/apps/demo_bin.cpp +++ b/apps/demo_bin.cpp @@ -108,9 +108,19 @@ struct KinFuApp { cuda::DeviceArray cloud = kinfu.tsdf().fetchCloud(cloud_buffer); cv::Mat cloud_host(1, (int)cloud.size(), CV_32FC4); - cloud.download(cloud_host.ptr()); - viz.showWidget("cloud", cv::viz::WCloud(cloud_host)); - //viz.showWidget("cloud", cv::viz::WPaintedCloud(cloud_host)); + + if (kinfu.params().integrate_color) { + kinfu.color_volume()->fetchColors(cloud, color_buffer); + cv::Mat color_host(1, (int)cloud.size(), CV_8UC4); + cloud.download(cloud_host.ptr()); + color_buffer.download(color_host.ptr()); + viz.showWidget("cloud", cv::viz::WCloud(cloud_host, color_host)); + } else + { + cloud.download(cloud_host.ptr()); + viz.showWidget("cloud", cv::viz::WCloud(cloud_host)); + //viz.showWidget("cloud", cv::viz::WPaintedCloud(cloud_host)); + } } /** @@ -143,7 +153,8 @@ struct KinFuApp show_raycasted(kinfu); show_depth(depth); - //cv::imshow("Image", image); + if (kinfu.params().integrate_color) + cv::imshow("Image", image); if (!interactive_mode_) viz.setViewerPose(kinfu.getCameraPose()); @@ -185,6 +196,8 @@ struct KinFuApp cuda::Depth depth_device_; /**< */ cuda::DeviceArray cloud_buffer; + /**< */ + cuda::DeviceArray color_buffer; }; @@ -202,7 +215,8 @@ int main (int argc, char* argv[]) BinSource capture(argv[1], argv[2]); KinFuParams custom_params = KinFuParams::default_params(); - custom_params.volume_dims = Vec3i::all(768); + custom_params.integrate_color = true; + custom_params.volume_dims = Vec3i::all(256); custom_params.volume_size = Vec3f::all(0.7f); custom_params.volume_pose = Affine3f().translate(Vec3f(-custom_params.volume_size[0]/2, -custom_params.volume_size[1]/2, 0.5f)); custom_params.intr = Intr(520.89, 520.23, 324.54, 237.553); diff --git a/kfusion/include/kfusion/kinfu.hpp b/kfusion/include/kfusion/kinfu.hpp index ce89045..8e4bf35 100644 --- a/kfusion/include/kfusion/kinfu.hpp +++ b/kfusion/include/kfusion/kinfu.hpp @@ -68,6 +68,9 @@ namespace kfusion const cuda::TsdfVolume& tsdf() const; cuda::TsdfVolume& tsdf(); + const cv::Ptr color_volume() const; + cv::Ptr color_volume(); + const cuda::ProjectiveICP& icp() const; cuda::ProjectiveICP& icp(); diff --git a/kfusion/src/kinfu.cpp b/kfusion/src/kinfu.cpp index 442b736..8c1d831 100644 --- a/kfusion/src/kinfu.cpp +++ b/kfusion/src/kinfu.cpp @@ -16,6 +16,7 @@ kfusion::KinFuParams kfusion::KinFuParams::default_params() p.cols = 640; //pixels p.rows = 480; //pixels + p.integrate_color = false; p.intr = Intr(525.f, 525.f, p.cols/2 - 0.5f, p.rows/2 - 0.5f); p.volume_dims = Vec3i::all(512); //number of voxels @@ -35,6 +36,8 @@ kfusion::KinFuParams kfusion::KinFuParams::default_params() p.tsdf_trunc_dist = 0.04f; //meters; p.tsdf_max_weight = 64; //frames + p.color_max_weight = 64; //frames + p.raycast_step_factor = 0.75f; //in voxel sizes p.gradient_delta_factor = 0.5f; //in voxel sizes @@ -48,14 +51,23 @@ kfusion::KinFu::KinFu(const KinFuParams& params) : frame_counter_(0), params_(pa { CV_Assert(params.volume_dims[0] % 32 == 0); - volume_ = cv::Ptr(new cuda::TsdfVolume(params_.volume_dims)); + tsdf_volume_ = cv::Ptr(new cuda::TsdfVolume(params_.volume_dims)); + + tsdf_volume_->setTruncDist(params_.tsdf_trunc_dist); + tsdf_volume_->setMaxWeight(params_.tsdf_max_weight); + tsdf_volume_->setSize(params_.volume_size); + tsdf_volume_->setPose(params_.volume_pose); + tsdf_volume_->setRaycastStepFactor(params_.raycast_step_factor); + tsdf_volume_->setGradientDeltaFactor(params_.gradient_delta_factor); - volume_->setTruncDist(params_.tsdf_trunc_dist); - volume_->setMaxWeight(params_.tsdf_max_weight); - volume_->setSize(params_.volume_size); - volume_->setPose(params_.volume_pose); - volume_->setRaycastStepFactor(params_.raycast_step_factor); - volume_->setGradientDeltaFactor(params_.gradient_delta_factor); + if (params.integrate_color) { + color_volume_ = cv::Ptr(new cuda::ColorVolume(params_.volume_dims)); + + color_volume_->setTruncDist(params_.tsdf_trunc_dist); + color_volume_->setMaxWeight(params_.color_max_weight); + color_volume_->setSize(params_.volume_size); + color_volume_->setPose(params_.volume_pose); + } icp_ = cv::Ptr(new cuda::ProjectiveICP()); icp_->setDistThreshold(params_.icp_dist_thres); @@ -73,14 +85,28 @@ kfusion::KinFuParams& kfusion::KinFu::params() { return params_; } const kfusion::cuda::TsdfVolume& kfusion::KinFu::tsdf() const -{ return *volume_; } +{ return *tsdf_volume_; } kfusion::cuda::TsdfVolume& kfusion::KinFu::tsdf() -{ return *volume_; } +{ return *tsdf_volume_; } const kfusion::cuda::ProjectiveICP& kfusion::KinFu::icp() const { return *icp_; } +const cv::Ptr kfusion::KinFu::color_volume () const +{ + if (params_.integrate_color) + return color_volume_; + return cv::Ptr(); +} + +cv::Ptr kfusion::KinFu::color_volume () +{ + if (params_.integrate_color) + return color_volume_; + return cv::Ptr(); +} + kfusion::cuda::ProjectiveICP& kfusion::KinFu::icp() { return *icp_; } @@ -130,7 +156,9 @@ void kfusion::KinFu::reset() poses_.clear(); poses_.reserve(30000); poses_.push_back(Affine3f::Identity()); - volume_->clear(); + tsdf_volume_->clear(); + if (params_.integrate_color) + color_volume_->clear(); } kfusion::Affine3f kfusion::KinFu::getCameraPose (int time) const @@ -140,7 +168,7 @@ kfusion::Affine3f kfusion::KinFu::getCameraPose (int time) const return poses_[time]; } -bool kfusion::KinFu::operator()(const kfusion::cuda::Depth& depth, const kfusion::cuda::Image& /*image*/) +bool kfusion::KinFu::operator()(const kfusion::cuda::Depth& depth, const kfusion::cuda::Image& image) { const KinFuParams& p = params_; const int LEVELS = icp_->getUsedLevelsNum(); @@ -166,7 +194,7 @@ bool kfusion::KinFu::operator()(const kfusion::cuda::Depth& depth, const kfusion //can't perform more on first frame if (frame_counter_ == 0) { - volume_->integrate(dists_, poses_.back(), p.intr); + tsdf_volume_->integrate(dists_, poses_.back(), p.intr); #if defined USE_DEPTH curr_.depth_pyr.swap(prev_.depth_pyr); #else @@ -202,7 +230,10 @@ bool kfusion::KinFu::operator()(const kfusion::cuda::Depth& depth, const kfusion if (integrate) { //ScopeTime time("tsdf"); - volume_->integrate(dists_, poses_.back(), p.intr); + tsdf_volume_->integrate(dists_, poses_.back(), p.intr); + if (p.integrate_color) { + //color_volume_->integrate(image, dists_, poses_.back(), p.intr); + } } /////////////////////////////////////////////////////////////////////////////////////////// @@ -214,7 +245,7 @@ bool kfusion::KinFu::operator()(const kfusion::cuda::Depth& depth, const kfusion for (int i = 1; i < LEVELS; ++i) resizeDepthNormals(prev_.depth_pyr[i-1], prev_.normals_pyr[i-1], prev_.depth_pyr[i], prev_.normals_pyr[i]); #else - volume_->raycast(poses_.back(), p.intr, prev_.points_pyr[0], prev_.normals_pyr[0]); + tsdf_volume_->raycast(poses_.back(), p.intr, prev_.points_pyr[0], prev_.normals_pyr[0]); for (int i = 1; i < LEVELS; ++i) resizePointsNormals(prev_.points_pyr[i-1], prev_.normals_pyr[i-1], prev_.points_pyr[i], prev_.normals_pyr[i]); #endif @@ -265,7 +296,7 @@ void kfusion::KinFu::renderImage(cuda::Image& image, const Affine3f& pose, int f #define PASS1 points_ #endif - volume_->raycast(pose, p.intr, PASS1, normals_); + tsdf_volume_->raycast(pose, p.intr, PASS1, normals_); if (flag < 1 || flag > 3) cuda::renderImage(PASS1, normals_, params_.intr, params_.light_pose, image); From cd969f337f0cafb3227300ae134c2610c9ef2e8e Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Mon, 4 Jul 2016 11:40:03 +0200 Subject: [PATCH 11/21] Sync changes to color_volume --- kfusion/src/color_volume.cpp | 16 +++++++++++++++- kfusion/src/cuda/color_volume.cu | 33 +++++++++++++++++++++++++++----- kfusion/src/cuda/device.hpp | 24 ++++++++++++++++++++++- kfusion/src/internal.hpp | 8 +++++--- 4 files changed, 71 insertions(+), 10 deletions(-) diff --git a/kfusion/src/color_volume.cpp b/kfusion/src/color_volume.cpp index 8a7dc76..2aea31d 100644 --- a/kfusion/src/color_volume.cpp +++ b/kfusion/src/color_volume.cpp @@ -108,5 +108,19 @@ void kfusion::cuda::ColorVolume::integrate(const Image& rgb_image, void kfusion::cuda::ColorVolume::fetchColors(const DeviceArray& cloud, DeviceArray& colors) const { - // @TODO: (Gabriel) Implement! (with corresponding CUDA kernel in color_volume.cu) + if (colors.empty ()) + colors.create (cloud.size()); + + DeviceArray& pts = (DeviceArray&)cloud; + PtrSz col(reinterpret_cast(colors.ptr()), colors.size()); + // DeviceArray& col = (DeviceArray&)colors; + + device::Vec3i dims = device_cast(dims_); + device::Vec3f vsz = device_cast(getVoxelSize()); + device::Aff3f aff = device_cast(pose_); + + device::ColorVolume volume((uchar4*)data_.ptr(), dims, vsz, trunc_dist_, max_weight_); + device::fetchColors(volume, pts, col); + +// return DeviceArray((Point*)cloud_buffer.ptr(), size); } \ No newline at end of file diff --git a/kfusion/src/cuda/color_volume.cu b/kfusion/src/cuda/color_volume.cu index 9c4d2b2..5b2e7f6 100644 --- a/kfusion/src/cuda/color_volume.cu +++ b/kfusion/src/cuda/color_volume.cu @@ -1,6 +1,7 @@ #include "device.hpp" #include "texture_binder.hpp" #include "../internal.hpp" +#include using namespace kfusion::device; @@ -63,6 +64,8 @@ namespace kfusion int x = blockIdx.x * blockDim.x + threadIdx.x; int y = blockIdx.y * blockDim.y + threadIdx.y; + printf("integrate"); + if (x >= volume.dims.x || y >= volume.dims.y) return; @@ -163,26 +166,46 @@ namespace kfusion { int idx = blockIdx.x * blockDim.x + threadIdx.x; + printf ("fetchColors_kernel \n"); +/* if (idx < points.size) { + printf("idx"); int3 v; float3 p = *(const float3 *) (points.data + idx); - v.x = __float2int_rd( - p.x / cell_size.x); // round to negative infinity + v.x = __float2int_rd(p.x / cell_size.x); // round to negative infinity v.y = __float2int_rd(p.y / cell_size.y); v.z = __float2int_rd(p.z / cell_size.z); - uchar4 rgbw = *volume(v.x, v.y, v.z); - colors[idx] = make_uchar4(rgbw.z, rgbw.y, rgbw.x, 0); //bgra + //uchar4 rgbw = *volume(v.x, v.y, v.z); + uchar4 *pix = colors.data; + if (pix == NULL) { + printf ("null\n"); + } + //pix[idx] = rgbw; //bgra + + //uchar4 rgbw = gmem::LdCs(volume(v.x, v.y, v.z)); + //gmem::StCs(rgbw, colors.data + idx); + + // DEBUG PURPOSE + //colors[idx] = make_uchar4(255, 0, 0, 0); //bgra } + */ } } } void -kfusion::device::fetchColors(const ColorVolume& volume, const PtrSz& points, PtrSz& colors) +kfusion::device::fetchColors(const ColorVolume& volume, const PtrSz& points, PtrSz& colors) { const int block = 256; + + // DEBUG PURPOSE + printf("[device::fetchColors] Debug: points.size = %d colors.size = %d", points.size, colors.size); + + if (points.size != colors.size) + return; + float3 cell_size = make_float3 (volume.voxel_size.x, volume.voxel_size.y, volume.voxel_size.z); fetchColors_kernel<<>>(cell_size, volume, points, colors); cudaSafeCall ( cudaGetLastError () ); diff --git a/kfusion/src/cuda/device.hpp b/kfusion/src/cuda/device.hpp index 5a252be..bb44066 100644 --- a/kfusion/src/cuda/device.hpp +++ b/kfusion/src/cuda/device.hpp @@ -112,6 +112,10 @@ namespace kfusion template<> __kf_device__ ushort2 gmem::LdCs(ushort2* ptr); template<> __kf_device__ void gmem::StCs(const ushort2& val, ushort2* ptr); + /* + template<> __kf_device__ uchar4 gmem::LdCs(uchar4* ptr); + template<> __kf_device__ void gmem::StCs(const uchar4& val, uchar4* ptr); + */ } } @@ -130,17 +134,35 @@ namespace kfusion asm("ld.global.cs.v2.u16 {%0, %1}, [%2];" : "=h"(reinterpret_cast(val.x)), "=h"(reinterpret_cast(val.y)) : _ASM_PTR_(ptr)); return val; } - +/* + template<> __kf_device__ uchar4 kfusion::device::gmem::LdCs(uchar4* ptr) + { + uchar4 val; + asm("ld.global.cs.v2.u8 {%0, %1, %2, %3}, [%4];" : "=h"(reinterpret_cast(val.x)), "=h"(reinterpret_cast(val.y)), "=h"(reinterpret_cast(val.z)), "=h"(reinterpret_cast(val.w)) : _ASM_PTR_(ptr)); + return val; + } +*/ template<> __kf_device__ void kfusion::device::gmem::StCs(const ushort2& val, ushort2* ptr) { short cx = val.x, cy = val.y; asm("st.global.cs.v2.u16 [%0], {%1, %2};" : : _ASM_PTR_(ptr), "h"(reinterpret_cast(cx)), "h"(reinterpret_cast(cy))); } +/* + template<> __kf_device__ void kfusion::device::gmem::StCs(const uchar4& val, uchar4* ptr) + { + uchar cx = val.x, cy = val.y, cz = val.z, cw = val.w; + asm("st.global.cs.v2.u8 [%0], {%1, %2, %3, %4};" : : _ASM_PTR_(ptr), cx, cy, cz, cw); + } + */ #undef _ASM_PTR_ #else template<> __kf_device__ ushort2 kfusion::device::gmem::LdCs(ushort2* ptr) { return *ptr; } template<> __kf_device__ void kfusion::device::gmem::StCs(const ushort2& val, ushort2* ptr) { *ptr = val; } +/* + template<> __kf_device__ uchar4 kfusion::device::gmem::LdCs(uchar4* ptr) { return *ptr; } + template<> __kf_device__ void kfusion::device::gmem::StCs(const uchar4& val, uchar4* ptr) { *ptr = val; } + */ #endif diff --git a/kfusion/src/internal.hpp b/kfusion/src/internal.hpp index abf4020..943f5ff 100644 --- a/kfusion/src/internal.hpp +++ b/kfusion/src/internal.hpp @@ -15,12 +15,14 @@ namespace kfusion typedef unsigned short ushort; typedef unsigned char uchar; + typedef uchar4 Color; + typedef PtrStepSz Dists; typedef DeviceArray2D Depth; typedef DeviceArray2D Normals; typedef DeviceArray2D Points; - typedef PtrStepSz Colors; // Not sure it's needed - typedef DeviceArray2D Image; + typedef PtrStepSz Colors; // Not sure it's needed + typedef DeviceArray2D Image; typedef int3 Vec3i; typedef float3 Vec3f; @@ -141,7 +143,7 @@ namespace kfusion //color volume functions void clear_volume(ColorVolume volume); void integrate(const Colors& image, const Dists& depth_map, ColorVolume& volume, const Aff3f& aff, const Projector& proj); - void fetchColors(const ColorVolume& volume, const PtrSz& points, PtrSz& colors); + void fetchColors(const ColorVolume& volume, const PtrSz& points, PtrSz& colors); //image proc functions void compute_dists(const Depth& depth, Dists dists, float2 f, float2 c); From fa519548cc3a925a99f5ca8d9f9ba30366a2ebb9 Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Wed, 6 Jul 2016 19:44:51 +0200 Subject: [PATCH 12/21] Fix small missing changes in order to integrate Colors Caution: There is still a bug in the fetchColors cuda kernel of color_volume.cu --- kfusion/CMakeLists.txt | 2 +- kfusion/src/bin_grabber.cpp | 4 ++-- kfusion/src/color_volume.cpp | 2 +- kfusion/src/cuda/color_volume.cu | 25 +++++++++++++------------ kfusion/src/kinfu.cpp | 2 +- 5 files changed, 18 insertions(+), 17 deletions(-) diff --git a/kfusion/CMakeLists.txt b/kfusion/CMakeLists.txt index 441f7dc..b78cc18 100644 --- a/kfusion/CMakeLists.txt +++ b/kfusion/CMakeLists.txt @@ -47,7 +47,7 @@ if(OPENNI_FOUND) include/io/capture.hpp) endif(OPENNI_FOUND) -include_directories(include src src/cuda) +include_directories(include include/kfusion/cuda/ src src/cuda) set(__has_cuda OFF) check_cuda(__has_cuda) diff --git a/kfusion/src/bin_grabber.cpp b/kfusion/src/bin_grabber.cpp index 85883ab..a608a2c 100644 --- a/kfusion/src/bin_grabber.cpp +++ b/kfusion/src/bin_grabber.cpp @@ -116,8 +116,8 @@ namespace kfusion { rgb_image_stream_.read((char*)(&(this->kinect_data_->rgb_frame[i])), 3); rgb_image_stream_.read(&alpha, 1); } - image.create(this->kinect_data_->rgb_frame_height, this->kinect_data_->rgb_frame_width, CV_8UC3); - memcpy(image.data, this->kinect_data_->rgb_frame.data(), 3 * rgb_frame_size); + image.create(this->kinect_data_->rgb_frame_height, this->kinect_data_->rgb_frame_width, CV_8UC4); + memcpy(image.data, this->kinect_data_->rgb_frame.data(), 4 * rgb_frame_size); success = true; ++cur_frame_; diff --git a/kfusion/src/color_volume.cpp b/kfusion/src/color_volume.cpp index 2aea31d..bd90b24 100644 --- a/kfusion/src/color_volume.cpp +++ b/kfusion/src/color_volume.cpp @@ -108,7 +108,7 @@ void kfusion::cuda::ColorVolume::integrate(const Image& rgb_image, void kfusion::cuda::ColorVolume::fetchColors(const DeviceArray& cloud, DeviceArray& colors) const { - if (colors.empty ()) + if (colors.size() != cloud.size()) colors.create (cloud.size()); DeviceArray& pts = (DeviceArray&)cloud; diff --git a/kfusion/src/cuda/color_volume.cu b/kfusion/src/cuda/color_volume.cu index 5b2e7f6..b1afbdf 100644 --- a/kfusion/src/cuda/color_volume.cu +++ b/kfusion/src/cuda/color_volume.cu @@ -64,8 +64,6 @@ namespace kfusion int x = blockIdx.x * blockDim.x + threadIdx.x; int y = blockIdx.y * blockDim.y + threadIdx.y; - printf("integrate"); - if (x >= volume.dims.x || y >= volume.dims.y) return; @@ -162,22 +160,24 @@ namespace kfusion { __global__ void fetchColors_kernel (const float3 cell_size, const ColorVolume &volume, - const PtrSz &points, PtrSz &colors) + const PtrSz &points, PtrSz &colors) { int idx = blockIdx.x * blockDim.x + threadIdx.x; - printf ("fetchColors_kernel \n"); -/* - if (idx < points.size) + printf ("fetchColors_kernel %d \n", idx); + + int n_pts = points.size; + if (idx < n_pts) { - printf("idx"); - int3 v; - float3 p = *(const float3 *) (points.data + idx); + printf("idx\n"); + } + /* int3 v; + float4 p = *(const float4 *) (points.data + idx); v.x = __float2int_rd(p.x / cell_size.x); // round to negative infinity v.y = __float2int_rd(p.y / cell_size.y); v.z = __float2int_rd(p.z / cell_size.z); - //uchar4 rgbw = *volume(v.x, v.y, v.z); + uchar4 rgbw = *volume(v.x, v.y, v.z); uchar4 *pix = colors.data; if (pix == NULL) { printf ("null\n"); @@ -190,7 +190,7 @@ namespace kfusion // DEBUG PURPOSE //colors[idx] = make_uchar4(255, 0, 0, 0); //bgra } - */ + */ } } } @@ -201,7 +201,8 @@ kfusion::device::fetchColors(const ColorVolume& volume, const PtrSz& poin const int block = 256; // DEBUG PURPOSE - printf("[device::fetchColors] Debug: points.size = %d colors.size = %d", points.size, colors.size); + printf("[device::fetchColors] Debug: points.size = %d colors.size = %d \n", + static_cast(points.size), static_cast(colors.size)); if (points.size != colors.size) return; diff --git a/kfusion/src/kinfu.cpp b/kfusion/src/kinfu.cpp index 8c1d831..00ce1f7 100644 --- a/kfusion/src/kinfu.cpp +++ b/kfusion/src/kinfu.cpp @@ -232,7 +232,7 @@ bool kfusion::KinFu::operator()(const kfusion::cuda::Depth& depth, const kfusion //ScopeTime time("tsdf"); tsdf_volume_->integrate(dists_, poses_.back(), p.intr); if (p.integrate_color) { - //color_volume_->integrate(image, dists_, poses_.back(), p.intr); + color_volume_->integrate(image, dists_, poses_.back(), p.intr); } } From 715d85b0045342551360d95bff87aab409b22e4e Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Thu, 7 Jul 2016 10:26:19 +0200 Subject: [PATCH 13/21] Fix memory access bug --- kfusion/src/cuda/color_volume.cu | 50 ++++++++++++++++++++++---------- 1 file changed, 35 insertions(+), 15 deletions(-) diff --git a/kfusion/src/cuda/color_volume.cu b/kfusion/src/cuda/color_volume.cu index b1afbdf..992b46d 100644 --- a/kfusion/src/cuda/color_volume.cu +++ b/kfusion/src/cuda/color_volume.cu @@ -158,21 +158,30 @@ namespace kfusion { namespace device { - __global__ void - fetchColors_kernel (const float3 cell_size, const ColorVolume &volume, - const PtrSz &points, PtrSz &colors) + struct ColorFetcher { - int idx = blockIdx.x * blockDim.x + threadIdx.x; + ColorVolume volume; - printf ("fetchColors_kernel %d \n", idx); + float3 cell_size; + int n_pts; + const float4* pts_data; - int n_pts = points.size; - if (idx < n_pts) + ColorFetcher(const ColorVolume& volume, float3 cell_size); + + __kf_device__ + void operator()(PtrSz colors) const { - printf("idx\n"); - } - /* int3 v; - float4 p = *(const float4 *) (points.data + idx); + int idx = blockIdx.x * blockDim.x + threadIdx.x; + + printf ("fetchColors_kernel %d \n", idx); + + if (idx < n_pts) + { + printf("idx\n"); + } + + int3 v; + float4 p = *(const float4 *) (pts_data + idx); v.x = __float2int_rd(p.x / cell_size.x); // round to negative infinity v.y = __float2int_rd(p.y / cell_size.y); v.z = __float2int_rd(p.z / cell_size.z); @@ -182,16 +191,22 @@ namespace kfusion if (pix == NULL) { printf ("null\n"); } - //pix[idx] = rgbw; //bgra + pix[idx] = rgbw; //bgra //uchar4 rgbw = gmem::LdCs(volume(v.x, v.y, v.z)); //gmem::StCs(rgbw, colors.data + idx); // DEBUG PURPOSE //colors[idx] = make_uchar4(255, 0, 0, 0); //bgra + } - */ - } + }; + + inline ColorFetcher::ColorFetcher(const ColorVolume& _volume, float3 _cell_size) + : volume(_volume), cell_size(_cell_size) {} + + __global__ void fetchColors_kernel (const ColorFetcher colorfetcher, PtrSz colors) + {colorfetcher(colors);}; } } @@ -208,7 +223,12 @@ kfusion::device::fetchColors(const ColorVolume& volume, const PtrSz& poin return; float3 cell_size = make_float3 (volume.voxel_size.x, volume.voxel_size.y, volume.voxel_size.z); - fetchColors_kernel<<>>(cell_size, volume, points, colors); + + ColorFetcher cf(volume, cell_size); + cf.n_pts = points.size; + cf.pts_data = points.data; + + fetchColors_kernel<<>>(cf, colors); cudaSafeCall ( cudaGetLastError () ); cudaSafeCall (cudaDeviceSynchronize ()); }; \ No newline at end of file From 01141a310199b4c795c3ac8f06121a59d12012d2 Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Mon, 11 Jul 2016 10:28:08 +0200 Subject: [PATCH 14/21] Add color image support in demo_bin app --- apps/demo_bin.cpp | 20 +++++++++++++----- kfusion/src/cuda/color_volume.cu | 36 ++++++++++++++------------------ 2 files changed, 31 insertions(+), 25 deletions(-) diff --git a/apps/demo_bin.cpp b/apps/demo_bin.cpp index c2902e4..74bf93c 100644 --- a/apps/demo_bin.cpp +++ b/apps/demo_bin.cpp @@ -107,6 +107,10 @@ struct KinFuApp void take_cloud(KinFu& kinfu) { cuda::DeviceArray cloud = kinfu.tsdf().fetchCloud(cloud_buffer); + + // DEBUG PURPOSE + std::cout << cloud.size() << std::endl; + cv::Mat cloud_host(1, (int)cloud.size(), CV_32FC4); if (kinfu.params().integrate_color) { @@ -143,18 +147,22 @@ struct KinFuApp return std::cout << "Can't grab" << std::endl, false; depth_device_.upload(depth.data, depth.step, depth.rows, depth.cols); + color_device_.upload(image.data, image.step, image.rows, image.cols); { SampledScopeTime fps(time_ms); (void)fps; - has_image = kinfu(depth_device_); + if (kinfu.params().integrate_color) + has_image = kinfu(depth_device_, color_device_); + else + has_image = kinfu(depth_device_); } if (has_image) show_raycasted(kinfu); show_depth(depth); - if (kinfu.params().integrate_color) - cv::imshow("Image", image); + //if (kinfu.params().integrate_color) + //cv::imshow("Image", image); if (!interactive_mode_) viz.setViewerPose(kinfu.getCameraPose()); @@ -194,6 +202,8 @@ struct KinFuApp cuda::Image view_device_; /**< Depth frame on the GPU */ cuda::Depth depth_device_; + /**< Color frame on the GPU */ + cuda::Image color_device_; /**< */ cuda::DeviceArray cloud_buffer; /**< */ @@ -216,11 +226,11 @@ int main (int argc, char* argv[]) KinFuParams custom_params = KinFuParams::default_params(); custom_params.integrate_color = true; - custom_params.volume_dims = Vec3i::all(256); + custom_params.volume_dims = Vec3i::all(64); custom_params.volume_size = Vec3f::all(0.7f); custom_params.volume_pose = Affine3f().translate(Vec3f(-custom_params.volume_size[0]/2, -custom_params.volume_size[1]/2, 0.5f)); custom_params.intr = Intr(520.89, 520.23, 324.54, 237.553); - custom_params.tsdf_trunc_dist = 0.005; + custom_params.tsdf_trunc_dist = 0.05; KinFuApp app (capture, custom_params); diff --git a/kfusion/src/cuda/color_volume.cu b/kfusion/src/cuda/color_volume.cu index 992b46d..6b0ac73 100644 --- a/kfusion/src/cuda/color_volume.cu +++ b/kfusion/src/cuda/color_volume.cu @@ -178,27 +178,23 @@ namespace kfusion if (idx < n_pts) { printf("idx\n"); - } - - int3 v; - float4 p = *(const float4 *) (pts_data + idx); - v.x = __float2int_rd(p.x / cell_size.x); // round to negative infinity - v.y = __float2int_rd(p.y / cell_size.y); - v.z = __float2int_rd(p.z / cell_size.z); - uchar4 rgbw = *volume(v.x, v.y, v.z); - uchar4 *pix = colors.data; - if (pix == NULL) { - printf ("null\n"); + int3 v; + float4 p = *(const float4 *) (pts_data + idx); + v.x = __float2int_rd(p.x / cell_size.x); // round to negative infinity + v.y = __float2int_rd(p.y / cell_size.y); + v.z = __float2int_rd(p.z / cell_size.z); + + uchar4 rgbw = *volume(v.x, v.y, v.z); + uchar4 *pix = colors.data; + if (pix == NULL) { + printf ("null\n"); + } + pix[idx] = rgbw; //bgra + + // DEBUG PURPOSE + //colors[idx] = make_uchar4(255, 0, 0, 0); //bgra } - pix[idx] = rgbw; //bgra - - //uchar4 rgbw = gmem::LdCs(volume(v.x, v.y, v.z)); - //gmem::StCs(rgbw, colors.data + idx); - - // DEBUG PURPOSE - //colors[idx] = make_uchar4(255, 0, 0, 0); //bgra - } }; @@ -219,7 +215,7 @@ kfusion::device::fetchColors(const ColorVolume& volume, const PtrSz& poin printf("[device::fetchColors] Debug: points.size = %d colors.size = %d \n", static_cast(points.size), static_cast(colors.size)); - if (points.size != colors.size) + if (points.size != colors.size || points.size == 0) return; float3 cell_size = make_float3 (volume.voxel_size.x, volume.voxel_size.y, volume.voxel_size.z); From c2399b339a20c8ff2d196dc6eb147972e5790b95 Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Mon, 11 Jul 2016 14:12:55 +0200 Subject: [PATCH 15/21] Add show image and remove useless debug output --- apps/demo_bin.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/apps/demo_bin.cpp b/apps/demo_bin.cpp index 74bf93c..3035cfd 100644 --- a/apps/demo_bin.cpp +++ b/apps/demo_bin.cpp @@ -108,9 +108,6 @@ struct KinFuApp { cuda::DeviceArray cloud = kinfu.tsdf().fetchCloud(cloud_buffer); - // DEBUG PURPOSE - std::cout << cloud.size() << std::endl; - cv::Mat cloud_host(1, (int)cloud.size(), CV_32FC4); if (kinfu.params().integrate_color) { @@ -161,8 +158,8 @@ struct KinFuApp show_raycasted(kinfu); show_depth(depth); - //if (kinfu.params().integrate_color) - //cv::imshow("Image", image); + if (kinfu.params().integrate_color) + cv::imshow("Image", image); if (!interactive_mode_) viz.setViewerPose(kinfu.getCameraPose()); @@ -226,7 +223,7 @@ int main (int argc, char* argv[]) KinFuParams custom_params = KinFuParams::default_params(); custom_params.integrate_color = true; - custom_params.volume_dims = Vec3i::all(64); + custom_params.volume_dims = Vec3i::all(256); custom_params.volume_size = Vec3f::all(0.7f); custom_params.volume_pose = Affine3f().translate(Vec3f(-custom_params.volume_size[0]/2, -custom_params.volume_size[1]/2, 0.5f)); custom_params.intr = Intr(520.89, 520.23, 324.54, 237.553); From 69c4f0e86ccb31a630c5880aa5130edbb2f643c8 Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Mon, 11 Jul 2016 14:24:26 +0200 Subject: [PATCH 16/21] Change bin_grabber such that it returns 4 channels matrix for RGBA color image --- kfusion/include/io/bin_grabber.hpp | 3 ++- kfusion/src/bin_grabber.cpp | 10 ++-------- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/kfusion/include/io/bin_grabber.hpp b/kfusion/include/io/bin_grabber.hpp index c611e4d..56cfa30 100644 --- a/kfusion/include/io/bin_grabber.hpp +++ b/kfusion/include/io/bin_grabber.hpp @@ -19,6 +19,7 @@ namespace kfusion { class KF_EXPORTS BinSource { public: typedef kfusion::PixelRGB RGB24; + typedef kfusion::RGB RGB32; struct KinectData { /**< Depth map width */ @@ -36,7 +37,7 @@ class KF_EXPORTS BinSource { /**< Size in bytes of one frame in the binary file */ int rgb_block_size; /**< Array of memory for the rgb image */ - std::vector rgb_frame; + std::vector rgb_frame; /** * @name KinectData diff --git a/kfusion/src/bin_grabber.cpp b/kfusion/src/bin_grabber.cpp index a608a2c..036cb58 100644 --- a/kfusion/src/bin_grabber.cpp +++ b/kfusion/src/bin_grabber.cpp @@ -9,9 +9,6 @@ #include #include -#include -#include -#include #include #include "io/bin_grabber.hpp" @@ -109,13 +106,10 @@ namespace kfusion { depth_image_stream_.read((char*)(&(this->kinect_data_->depth_frame[i])), 2); } - char alpha; int rgb_frame_size = this->kinect_data_->rgb_frame_width * this->kinect_data_->rgb_frame_height; this->kinect_data_->rgb_frame.resize(rgb_frame_size); - for (int i=0; i < rgb_frame_size; ++i) { - rgb_image_stream_.read((char*)(&(this->kinect_data_->rgb_frame[i])), 3); - rgb_image_stream_.read(&alpha, 1); - } + rgb_image_stream_.read((char*)(&(this->kinect_data_->rgb_frame[0])), 4 * rgb_frame_size); + image.create(this->kinect_data_->rgb_frame_height, this->kinect_data_->rgb_frame_width, CV_8UC4); memcpy(image.data, this->kinect_data_->rgb_frame.data(), 4 * rgb_frame_size); From 67a9abe419d34feb7ba153e103b9e253846d0be2 Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Mon, 11 Jul 2016 14:52:05 +0200 Subject: [PATCH 17/21] Add proper color integration --- kfusion/src/color_volume.cpp | 8 ++--- kfusion/src/cuda/color_volume.cu | 51 +++++++++++++------------------- kfusion/src/internal.hpp | 2 +- 3 files changed, 26 insertions(+), 35 deletions(-) diff --git a/kfusion/src/color_volume.cpp b/kfusion/src/color_volume.cpp index bd90b24..7b33ecf 100644 --- a/kfusion/src/color_volume.cpp +++ b/kfusion/src/color_volume.cpp @@ -115,12 +115,12 @@ void kfusion::cuda::ColorVolume::fetchColors(const DeviceArray& cloud, PtrSz col(reinterpret_cast(colors.ptr()), colors.size()); // DeviceArray& col = (DeviceArray&)colors; + Affine3f pose_inv = pose_.inv(); + device::Vec3i dims = device_cast(dims_); device::Vec3f vsz = device_cast(getVoxelSize()); - device::Aff3f aff = device_cast(pose_); + device::Aff3f aff_inv = device_cast(pose_inv); device::ColorVolume volume((uchar4*)data_.ptr(), dims, vsz, trunc_dist_, max_weight_); - device::fetchColors(volume, pts, col); - -// return DeviceArray((Point*)cloud_buffer.ptr(), size); + device::fetchColors(volume, aff_inv, pts, col); } \ No newline at end of file diff --git a/kfusion/src/cuda/color_volume.cu b/kfusion/src/cuda/color_volume.cu index 6b0ac73..e2254b5 100644 --- a/kfusion/src/cuda/color_volume.cu +++ b/kfusion/src/cuda/color_volume.cu @@ -47,7 +47,7 @@ namespace kfusion { namespace device { - texture image_tex(0, cudaFilterModePoint, cudaAddressModeBorder, cudaCreateChannelDescHalf()); + texture image_tex(0, cudaFilterModePoint, cudaAddressModeBorder); texture depth_tex(0, cudaFilterModePoint, cudaAddressModeBorder, cudaCreateChannelDescHalf()); struct ColorIntegrator { @@ -92,23 +92,23 @@ namespace kfusion { // Read the existing value and weight uchar4 volume_rgbw = *vptr; - int weight_prev = volume_rgbw.w; + uchar weight_prev = volume_rgbw.w; // Average with new value and weight uchar4 rgb = tex2D(image_tex, coo.x, coo.y); - const float Wrk = 1.f; - float new_x = (volume_rgbw.x * weight_prev + Wrk * rgb.x) / (weight_prev + Wrk); - float new_y = (volume_rgbw.y * weight_prev + Wrk * rgb.y) / (weight_prev + Wrk); - float new_z = (volume_rgbw.z * weight_prev + Wrk * rgb.z) / (weight_prev + Wrk); + uchar Wrk = 1; + uchar new_x = (volume_rgbw.x * weight_prev + Wrk * rgb.x) / (weight_prev + Wrk); + uchar new_y = (volume_rgbw.y * weight_prev + Wrk * rgb.y) / (weight_prev + Wrk); + uchar new_z = (volume_rgbw.z * weight_prev + Wrk * rgb.z) / (weight_prev + Wrk); - int weight_new = weight_prev + 1; + uchar weight_new = min(weight_prev + 1, 255); uchar4 volume_rgbw_new; - volume_rgbw_new.x = min (255, max (0, __float2int_rn (new_x))); - volume_rgbw_new.y = min (255, max (0, __float2int_rn (new_y))); - volume_rgbw_new.z = min (255, max (0, __float2int_rn (new_z))); - volume_rgbw_new.w = min (volume.max_weight, weight_new); + volume_rgbw_new.x = min(255, new_x); + volume_rgbw_new.y = min(255, new_y); + volume_rgbw_new.z = min(255, new_z); + volume_rgbw_new.w = min(volume.max_weight, weight_new); // Write back *vptr = volume_rgbw_new; @@ -138,7 +138,8 @@ void kfusion::device::integrate(const PtrStepSz& rgb_image, image_tex.addressMode[0] = cudaAddressModeBorder; image_tex.addressMode[1] = cudaAddressModeBorder; image_tex.addressMode[2] = cudaAddressModeBorder; - TextureBinder image_binder(rgb_image, image_tex, cudaCreateChannelDescHalf()); (void)image_binder; + cudaChannelFormatDesc channelDesc = cudaCreateChannelDesc(); + TextureBinder image_binder(rgb_image, image_tex, channelDesc); (void)image_binder; depth_tex.filterMode = cudaFilterModePoint; depth_tex.addressMode[0] = cudaAddressModeBorder; @@ -165,6 +166,7 @@ namespace kfusion float3 cell_size; int n_pts; const float4* pts_data; + Aff3f aff_inv; ColorFetcher(const ColorVolume& volume, float3 cell_size); @@ -173,27 +175,19 @@ namespace kfusion { int idx = blockIdx.x * blockDim.x + threadIdx.x; - printf ("fetchColors_kernel %d \n", idx); - if (idx < n_pts) { - printf("idx\n"); - int3 v; float4 p = *(const float4 *) (pts_data + idx); - v.x = __float2int_rd(p.x / cell_size.x); // round to negative infinity - v.y = __float2int_rd(p.y / cell_size.y); - v.z = __float2int_rd(p.z / cell_size.z); + float3 px = make_float3(p.x, p.y, p.z); + float3 pv = aff_inv * px; + v.x = __float2int_rd(pv.x / cell_size.x); // round to negative infinity + v.y = __float2int_rd(pv.y / cell_size.y); + v.z = __float2int_rd(pv.z / cell_size.z); uchar4 rgbw = *volume(v.x, v.y, v.z); uchar4 *pix = colors.data; - if (pix == NULL) { - printf ("null\n"); - } pix[idx] = rgbw; //bgra - - // DEBUG PURPOSE - //colors[idx] = make_uchar4(255, 0, 0, 0); //bgra } } }; @@ -207,14 +201,10 @@ namespace kfusion } void -kfusion::device::fetchColors(const ColorVolume& volume, const PtrSz& points, PtrSz& colors) +kfusion::device::fetchColors(const ColorVolume& volume, const Aff3f& aff_inv, const PtrSz& points, PtrSz& colors) { const int block = 256; - // DEBUG PURPOSE - printf("[device::fetchColors] Debug: points.size = %d colors.size = %d \n", - static_cast(points.size), static_cast(colors.size)); - if (points.size != colors.size || points.size == 0) return; @@ -223,6 +213,7 @@ kfusion::device::fetchColors(const ColorVolume& volume, const PtrSz& poin ColorFetcher cf(volume, cell_size); cf.n_pts = points.size; cf.pts_data = points.data; + cf.aff_inv = aff_inv; fetchColors_kernel<<>>(cf, colors); cudaSafeCall ( cudaGetLastError () ); diff --git a/kfusion/src/internal.hpp b/kfusion/src/internal.hpp index 943f5ff..f95d025 100644 --- a/kfusion/src/internal.hpp +++ b/kfusion/src/internal.hpp @@ -143,7 +143,7 @@ namespace kfusion //color volume functions void clear_volume(ColorVolume volume); void integrate(const Colors& image, const Dists& depth_map, ColorVolume& volume, const Aff3f& aff, const Projector& proj); - void fetchColors(const ColorVolume& volume, const PtrSz& points, PtrSz& colors); + void fetchColors(const ColorVolume& volume, const Aff3f& aff_inv, const PtrSz& points, PtrSz& colors); //image proc functions void compute_dists(const Depth& depth, Dists dists, float2 f, float2 c); From 71014489e5f572af50dc5f5f5ac4870125819acb Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Mon, 11 Jul 2016 15:46:02 +0200 Subject: [PATCH 18/21] Change .gitignore --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 5f73a11..fa6b67d 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +.gitignore .idea/ build/ - +build_xcode/ From ef8ec5df78bab9d50df0974cd7ed797e57c8eb39 Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Mon, 11 Jul 2016 16:56:35 +0200 Subject: [PATCH 19/21] Change image matrix format from CV_8UC3 to CV_8UC4 in order to keep the alignment on the GPU --- kfusion/src/capture.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/kfusion/src/capture.cpp b/kfusion/src/capture.cpp index f7e47dd..53b6f21 100644 --- a/kfusion/src/capture.cpp +++ b/kfusion/src/capture.cpp @@ -162,7 +162,7 @@ void kfusion::OpenNISource::open(const std::string& filename, bool repeat /*= fa sprintf (impl_->strError, "Open failed: %s\n", xnGetStatusString (rc)); REPORT_ERROR (impl_->strError); } - + impl_->player_.SetRepeat(repeat); rc = impl_->context.FindExistingNode (XN_NODE_TYPE_DEPTH, impl_->depth); @@ -231,11 +231,11 @@ bool kfusion::OpenNISource::grab(cv::Mat& depth, cv::Mat& image) const XnRGB24Pixel* pImage = impl_->imageMD.RGB24Data (); int x = impl_->imageMD.FullXRes (); int y = impl_->imageMD.FullYRes (); - image.create(y, x, CV_8UC3); + image.create(y, x, CV_8UC4); - cv::Vec3b *dptr = image.ptr(); + cv::Vec4b *dptr = image.ptr(); for(size_t i = 0; i < image.total(); ++i) - dptr[i] = cv::Vec3b(pImage[i].nBlue, pImage[i].nGreen, pImage[i].nRed); + dptr[i] = cv::Vec4b(pImage[i].nBlue, pImage[i].nGreen, pImage[i].nRed, 0.0); } else { From 4d23ad682f072ee29dc4fba5b85d0e968f54c05e Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Mon, 11 Jul 2016 17:13:18 +0200 Subject: [PATCH 20/21] Add depth alignement to RGB camera option (provided by OpenNI) --- kfusion/src/capture.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/kfusion/src/capture.cpp b/kfusion/src/capture.cpp index 53b6f21..4e08773 100644 --- a/kfusion/src/capture.cpp +++ b/kfusion/src/capture.cpp @@ -130,6 +130,13 @@ void kfusion::OpenNISource::open (int device) { impl_->has_image = true; rc = impl_->image.SetMapOutputMode (mode); + rc = impl_->depth.GetAlternativeViewPointCap().SetViewPoint(impl_->image); + if (rc != XN_STATUS_OK) + { + sprintf(impl_->strError, "Depth alignment failed: %s\n", + xnGetStatusString(rc)); + REPORT_ERROR (impl_->strError); + } } getParams (); From e6f4f151a2ea5b4fe9210c2a2f0149dcc63fd0da Mon Sep 17 00:00:00 2001 From: Gabriel Cuendet Date: Mon, 11 Jul 2016 17:14:41 +0200 Subject: [PATCH 21/21] Add alternative constructor to KinFuApp (taking custom parameters) and integrate colors in the demo --- apps/demo.cpp | 42 ++++++++++++++++++++++++++++++++++++------ 1 file changed, 36 insertions(+), 6 deletions(-) diff --git a/apps/demo.cpp b/apps/demo.cpp index ebbd980..eabe811 100644 --- a/apps/demo.cpp +++ b/apps/demo.cpp @@ -36,6 +36,18 @@ struct KinFuApp viz.registerKeyboardCallback(KeyboardCallback, this); } + KinFuApp(OpenNISource& source, const KinFuParams& params) : exit_ (false), iteractive_mode_(false), capture_ (source), pause_(false) + { + kinfu_ = KinFu::Ptr( new KinFu(params) ); + + capture_.setRegistration(true); + + cv::viz::WCube cube(cv::Vec3d::all(0), cv::Vec3d(params.volume_size), true, cv::viz::Color::apricot()); + viz.showWidget("cube", cube, params.volume_pose); + viz.showWidget("coor", cv::viz::WCoordinateSystem(0.1)); + viz.registerKeyboardCallback(KeyboardCallback, this); + } + void show_depth(const cv::Mat& depth) { cv::Mat display; @@ -61,9 +73,17 @@ struct KinFuApp { cuda::DeviceArray cloud = kinfu.tsdf().fetchCloud(cloud_buffer); cv::Mat cloud_host(1, (int)cloud.size(), CV_32FC4); - cloud.download(cloud_host.ptr()); - viz.showWidget("cloud", cv::viz::WCloud(cloud_host)); - //viz.showWidget("cloud", cv::viz::WPaintedCloud(cloud_host)); + if (kinfu.params().integrate_color) { + kinfu.color_volume()->fetchColors(cloud, color_buffer); + cv::Mat color_host(1, (int)cloud.size(), CV_8UC4); + cloud.download(cloud_host.ptr()); + color_buffer.download(color_host.ptr()); + viz.showWidget("cloud", cv::viz::WCloud(cloud_host, color_host)); + } else + { + cloud.download(cloud_host.ptr()); + viz.showWidget("cloud", cv::viz::WCloud(cloud_host)); + } } bool execute() @@ -80,17 +100,22 @@ struct KinFuApp return std::cout << "Can't grab" << std::endl, false; depth_device_.upload(depth.data, depth.step, depth.rows, depth.cols); + color_device_.upload(image.data, image.step, image.rows, image.cols); { SampledScopeTime fps(time_ms); (void)fps; - has_image = kinfu(depth_device_); + if (kinfu.params().integrate_color) + has_image = kinfu(depth_device_, color_device_); + else + has_image = kinfu(depth_device_); } if (has_image) show_raycasted(kinfu); show_depth(depth); - //cv::imshow("Image", image); + if (kinfu.params().integrate_color) + cv::imshow("Image", image); if (!iteractive_mode_) viz.setViewerPose(kinfu.getCameraPose()); @@ -120,7 +145,9 @@ struct KinFuApp cv::Mat view_host_; cuda::Image view_device_; cuda::Depth depth_device_; + cuda::Image color_device_; cuda::DeviceArray cloud_buffer; + cuda::DeviceArray color_buffer; }; @@ -145,7 +172,10 @@ int main (int argc, char* argv[]) //capture.open("d:/onis/20111013-224551.oni"); //capture.open("d:/onis/20111013-224719.oni"); - KinFuApp app (capture); + KinFuParams custom_params = KinFuParams::default_params(); + custom_params.integrate_color = true; + + KinFuApp app (capture, custom_params); // executing try { app.execute (); }