From 13d588db26b016af8e4b9537825233972c6d94c6 Mon Sep 17 00:00:00 2001 From: ZouYuhua Date: Wed, 11 Jun 2014 12:03:12 +0800 Subject: [PATCH 1/2] Add mesh generation and saving functions --- CMakeLists.txt | 3 +- apps/CMakeLists.txt | 2 +- apps/demo.cpp | 370 ++++++++++++++++++++++++++++---------------- 3 files changed, 238 insertions(+), 137 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c451ee3..dd3e4ef 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,8 @@ include(cmake/Targets.cmake) find_package(OpenCV 2.4.8 REQUIRED COMPONENTS core viz highgui) find_package(CUDA 5.0 REQUIRED) find_package(OpenNI2 REQUIRED) -include_directories(${OpenCV_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS} ${OPENNI2_INCLUDE_DIRS}) +find_package(PCL 1.7.0 REQUIRED COMPONENTS common io gpu_kinfu_large_scale) +include_directories(${OpenCV_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS} ${OPENNI2_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) # ---[ misc settings if(USE_PROJECT_FOLDERS) diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index 33731f5..9983d2c 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -2,7 +2,7 @@ include_directories(${CMAKE_SOURCE_DIR}/kfusion/include) file(GLOB srcs *.cpp *.hpp) add_executable(demo ${srcs}) -target_link_libraries(demo ${OpenCV_LIBS} kfusion) +target_link_libraries(demo ${OpenCV_LIBS} ${PCL_LIBRARIES} kfusion) set_target_properties(demo PROPERTIES DEBUG_POSTFIX "d" diff --git a/apps/demo.cpp b/apps/demo.cpp index ad1b9c2..3ad446b 100644 --- a/apps/demo.cpp +++ b/apps/demo.cpp @@ -1,4 +1,14 @@ #include + +#include +#include + +#include +#include +#include +#include +#include + #include #include #include @@ -7,118 +17,208 @@ 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(OpenNISource& source) : exit_ (false), iteractive_mode_(false), capture_ (source) - { - 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(3); - - switch(key) - { - case 't': case 'T' : take_cloud(kinfu); break; - case 'i': case 'I' : iteractive_mode_ = !iteractive_mode_; break; - case 27: case 32: exit_ = true; break; - } - - //exit_ = exit_ || i > 100; - viz.spinOnce(3, true); - } - return true; - } - - bool exit_, iteractive_mode_; - OpenNISource& capture_; - KinFu::Ptr kinfu_; - cv::viz::Viz3d viz; - - cv::Mat view_host_; - cuda::Image view_device_; - cuda::Depth depth_device_; - cuda::DeviceArray cloud_buffer; + 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(OpenNISource& source) : + exit_ (false), iteractive_mode_(false), capture_ (source) + { + 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)); + } + + void generate_mesh(KinFu& kinfu) + { + std::cout << "\nGetting mesh... " << std::endl; + + ////////////////////////////////////////////////////////////////////////// + // Download point clouds from device + Vec3i dims = kinfu.tsdf().getDims(); + if (!cloud_buffer.empty()) + cloud_buffer.release(); + cloud_buffer.create(dims[0]*dims[1]*dims[2]); + cuda::DeviceArray cloud_device = kinfu.tsdf().fetchCloud(cloud_buffer); + + pcl::PointCloud::Ptr cloud_pcl (new pcl::PointCloud); + + cv::Mat cloud_host(1, (int)cloud_device.size(), CV_32FC4); + cloud_device.download(cloud_host.ptr()); + //cloud_device.download(cloud->begin()); + + //#pragma omp parallel for + for (size_t i=0;i(0,i); + pnt.x = pnt_host[0]; + pnt.y = pnt_host[1]; + pnt.z = pnt_host[2]; + cloud_pcl->push_back(pnt); + } + + ////////////////////////////////////////////////////////////////////////// + // Normal estimation* + pcl::NormalEstimation n; + pcl::PointCloud::Ptr normals (new pcl::PointCloud); + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree); + tree->setInputCloud (cloud_pcl); + n.setInputCloud (cloud_pcl); + n.setSearchMethod (tree); + n.setKSearch (50); + n.compute (*normals); + //* normals should not contain the point normals + surface curvatures + + // Concatenate the XYZ and normal fields* + pcl::PointCloud::Ptr cloud_with_normals (new pcl::PointCloud); + pcl::concatenateFields (*cloud_pcl, *normals, *cloud_with_normals); + //* cloud_with_normals = cloud + normals + + //////////////////////////////////////////////////////////////////////// + // Create search tree* + pcl::search::KdTree::Ptr tree2 (new pcl::search::KdTree); + tree2->setInputCloud (cloud_with_normals); + + // Initialize objects + pcl::GreedyProjectionTriangulation gp3; + + // Set the maximum distance between connected points (maximum edge length) + gp3.setSearchRadius (0.1); + + // Set typical values for the parameters + gp3.setMu (2.5); + gp3.setMaximumNearestNeighbors (500); + gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees + gp3.setMinimumAngle(M_PI/18); // 10 degrees + gp3.setMaximumAngle(2*M_PI/3); // 120 degrees + gp3.setNormalConsistency(false); + + // Get result + gp3.setInputCloud (cloud_with_normals); + gp3.setSearchMethod (tree2); + gp3.reconstruct (mesh_); + + } + + void write_mesh(void) + { + if (!mesh_.polygons.empty()) + { + std::cout << "Saving mesh to 'mesh.ply'... " << std::endl; + pcl::io::savePLYFile("./mesh.ply", mesh_); + } + else + { + std::cerr << "Mesh is empty !" << std::endl; + } + } + + 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(3); + + switch(key) + { + case 't': case 'T' : take_cloud(kinfu); break; + case 'i': case 'I' : iteractive_mode_ = !iteractive_mode_; break; + case 'a': case 'A' : generate_mesh(kinfu); break; + case 's': case 'S' : write_mesh(); break; + case 27: case 32: exit_ = true; break; + } + + //exit_ = exit_ || i > 100; + viz.spinOnce(3, true); + } + return true; + } + + bool exit_, iteractive_mode_; + OpenNISource& capture_; + KinFu::Ptr kinfu_; + cv::viz::Viz3d viz; + + cv::Mat view_host_; + cuda::Image view_device_; + cuda::Depth depth_device_; + cuda::DeviceArray cloud_buffer; + + pcl::PolygonMesh mesh_; }; @@ -126,29 +226,29 @@ struct KinFuApp 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; - - OpenNISource capture; - capture.open (0); - //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"); - - 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; + 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; + + OpenNISource capture; + capture.open (0); + //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"); + + 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 7cfbc9c084838cea26435cd689ee55de8f11b6e6 Mon Sep 17 00:00:00 2001 From: ZouYuhua Date: Thu, 12 Jun 2014 14:00:32 +0800 Subject: [PATCH 2/2] Support importing data from .oni files --- apps/demo.cpp | 14 ++++++-------- kfusion/src/capture.cpp | 37 +++++++++++++++++++++++-------------- 2 files changed, 29 insertions(+), 22 deletions(-) diff --git a/apps/demo.cpp b/apps/demo.cpp index 3ad446b..1619dc4 100644 --- a/apps/demo.cpp +++ b/apps/demo.cpp @@ -234,14 +234,12 @@ int main (int argc, char* argv[]) 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; OpenNISource capture; - capture.open (0); - //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"); + if (argc == 2) { + capture.open(argv[1]); + } + else { + capture.open (0); + } KinFuApp app (capture); diff --git a/kfusion/src/capture.cpp b/kfusion/src/capture.cpp index 737063e..7ba7fba 100644 --- a/kfusion/src/capture.cpp +++ b/kfusion/src/capture.cpp @@ -48,6 +48,8 @@ void kfusion::OpenNISource::open (int device) // uri can be a device ID or filename void kfusion::OpenNISource::open(const std::string& filename) { + bool oniPlayback = false; + impl_ = cv::Ptr( new Impl () ); openni::Status rc; @@ -60,8 +62,11 @@ void kfusion::OpenNISource::open(const std::string& filename) REPORT_ERROR (impl_->strError); } - if (filename.length() > 0) + if (filename.length() > 0) + { rc = impl_->device.open( filename.c_str() ); + oniPlayback = true; + } else rc = impl_->device.open(openni::ANY_DEVICE); @@ -97,13 +102,6 @@ void kfusion::OpenNISource::open(const std::string& filename) REPORT_ERROR (impl_->strError); } - rc = impl_->colorStream.setVideoMode(colorMode); - if (rc != openni::STATUS_OK) - { - sprintf (impl_->strError, "Init failed: %s\n", openni::OpenNI::getExtendedError() ); - REPORT_ERROR (impl_->strError); - } - rc = impl_->depthStream.create(impl_->device, openni::SENSOR_DEPTH); if (rc != openni::STATUS_OK) { @@ -111,12 +109,23 @@ void kfusion::OpenNISource::open(const std::string& filename) REPORT_ERROR (impl_->strError); } - rc = impl_->depthStream.setVideoMode(depthMode); - if (rc != openni::STATUS_OK) - { - sprintf (impl_->strError, "Init failed: %s\n", openni::OpenNI::getExtendedError() ); - REPORT_ERROR (impl_->strError); - } + + if (!oniPlayback) + { + rc = impl_->colorStream.setVideoMode(colorMode); + if (rc != openni::STATUS_OK) + { + sprintf (impl_->strError, "Init failed: %s\n", openni::OpenNI::getExtendedError() ); + REPORT_ERROR (impl_->strError); + } + + rc = impl_->depthStream.setVideoMode(depthMode); + if (rc != openni::STATUS_OK) + { + sprintf (impl_->strError, "Init failed: %s\n", openni::OpenNI::getExtendedError() ); + REPORT_ERROR (impl_->strError); + } + } getParams ();