diff --git a/examples/protonect/CMakeLists.txt b/examples/protonect/CMakeLists.txt index b56c89c49..e8485a336 100644 --- a/examples/protonect/CMakeLists.txt +++ b/examples/protonect/CMakeLists.txt @@ -39,9 +39,12 @@ SET(LIBRARY_OUTPUT_PATH ${MY_DIR}/lib) # dependencies FIND_PACKAGE(PkgConfig) # try find PKGConfig as it will be used if found +FIND_PACKAGE(OpenCV) FIND_PACKAGE(LibUSB REQUIRED) -FIND_PACKAGE(OpenCV REQUIRED) FIND_PACKAGE(TurboJPEG REQUIRED) #does not provide a package-config file +IF(OPENCV_FOUND) + SET(LIBFREENECT2_OPENCV_FOUND 1) +ENDIF() # Add includes INCLUDE_DIRECTORIES( @@ -103,7 +106,6 @@ SET(SOURCES ) SET(LIBRARIES - ${OpenCV_LIBS} ${OpenCV_LIBRARIES} ${LibUSB_LIBRARIES} ${TurboJPEG_LIBRARIES} @@ -183,8 +185,21 @@ ENDIF() MESSAGE("Linking with these libraries: ${LIBRARIES}") TARGET_LINK_LIBRARIES(freenect2shared ${LIBRARIES}) +SET(Protonect_src + Protonect.cpp + ) + +IF (ENABLE_OPENGL AND OPENGL_FOUND) + LIST(APPEND Protonect_src + viewer.h + viewer.cpp + src/flextGL.c + ) +ENDIF() + ADD_EXECUTABLE(Protonect - Protonect.cpp + ${Protonect_src} + ) TARGET_LINK_LIBRARIES(Protonect diff --git a/examples/protonect/Protonect.cpp b/examples/protonect/Protonect.cpp index 3308fd0f7..943fd2309 100644 --- a/examples/protonect/Protonect.cpp +++ b/examples/protonect/Protonect.cpp @@ -28,13 +28,19 @@ #include #include -#include - #include #include #include #include #include +#ifdef LIBFREENECT2_WITH_OPENGL_SUPPORT +#include "viewer.h" +#endif + +#ifdef LIBFREENECT2_OPENCV_FOUND +#include +#endif + bool protonect_shutdown = false; @@ -135,6 +141,9 @@ int main(int argc, char *argv[]) libfreenect2::Registration* registration = new libfreenect2::Registration(dev->getIrCameraParams(), dev->getColorCameraParams()); + Viewer viewer; + viewer.initialize(); + while(!protonect_shutdown) { listener.waitForNewFrame(frames); @@ -142,17 +151,24 @@ int main(int argc, char *argv[]) libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir]; libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth]; + registration->apply(rgb, depth, &undistorted, ®istered); + +#if defined(LIBFREENECT2_WITH_OPENGL_SUPPORT) && !defined(LIBFREENECT2_OPENCV_FOUND) + viewer.addFrame("RGB", rgb); + viewer.addFrame("ir", ir); + viewer.addFrame("depth", depth); + viewer.addFrame("registered", ®istered); + + protonect_shutdown = viewer.render(); +#else cv::imshow("rgb", cv::Mat(rgb->height, rgb->width, CV_8UC4, rgb->data)); cv::imshow("ir", cv::Mat(ir->height, ir->width, CV_32FC1, ir->data) / 20000.0f); cv::imshow("depth", cv::Mat(depth->height, depth->width, CV_32FC1, depth->data) / 4500.0f); - - registration->apply(rgb,depth,&undistorted,®istered); - cv::imshow("undistorted", cv::Mat(undistorted.height, undistorted.width, CV_32FC1, undistorted.data) / 4500.0f); cv::imshow("registered", cv::Mat(registered.height, registered.width, CV_8UC4, registered.data)); - int key = cv::waitKey(1); protonect_shutdown = protonect_shutdown || (key > 0 && ((key & 0xFF) == 27)); // shutdown on escape +#endif listener.release(frames); //libfreenect2::this_thread::sleep_for(libfreenect2::chrono::milliseconds(100)); diff --git a/examples/protonect/include/libfreenect2/config.h.in b/examples/protonect/include/libfreenect2/config.h.in index f3abf7d43..3ee715bd2 100644 --- a/examples/protonect/include/libfreenect2/config.h.in +++ b/examples/protonect/include/libfreenect2/config.h.in @@ -47,4 +47,6 @@ #cmakedefine LIBFREENECT2_THREADING_TINYTHREAD +#cmakedefine LIBFREENECT2_OPENCV_FOUND + #endif // LIBFREENECT2_CONFIG_H diff --git a/examples/protonect/src/cpu_depth_packet_processor.cpp b/examples/protonect/src/cpu_depth_packet_processor.cpp index 2638f0607..6ac4624a8 100644 --- a/examples/protonect/src/cpu_depth_packet_processor.cpp +++ b/examples/protonect/src/cpu_depth_packet_processor.cpp @@ -28,7 +28,6 @@ #include #include -#include #include #include @@ -39,6 +38,150 @@ #include #endif +#include +#include + +template +struct Vec +{ + ScalarT val[Size]; +}; + +template +struct Mat +{ +private: + bool owns_buffer; + unsigned char *buffer_, *buffer_end_; + int width_, height_, x_step, y_step; + + void allocate(int width, int height, unsigned char *external_buffer = 0) + { + this->width_ = width; + this->height_ = height; + x_step = sizeof(ScalarT); + y_step = width * x_step; + + owns_buffer = external_buffer == 0; + + if(owns_buffer) + { + buffer_ = new unsigned char[y_step * height]; + } + else + { + buffer_ = external_buffer; + } + buffer_end_ = buffer_ + (y_step * height); + } + + void deallocate() + { + if(owns_buffer && buffer_ != 0) + { + delete[] buffer_; + owns_buffer = false; + buffer_ = 0; + buffer_end_ = 0; + } + } + +public: + Mat() + { + } + + Mat(int height, int width) : owns_buffer(false), buffer_(0) + { + create(height, width); + } + + template + Mat(int height, int width, DataT *external_buffer) + { + allocate(width, height, reinterpret_cast(external_buffer)); + } + + ~Mat() + { + deallocate(); + } + + int width() const + { + return width_; + } + + int height() const + { + return height_; + } + + void create(int height, int width) + { + deallocate(); + allocate(width, height); + } + + void copyTo(Mat &other) const + { + other.create(height(), width()); + std::copy(buffer_, buffer_end_, other.buffer_); + } + + const ScalarT &at(int y, int x) const + { + return *ptr(y, x); + } + + ScalarT &at(int y, int x) + { + return *ptr(y, x); + } + + const ScalarT *ptr(int y, int x) const + { + return reinterpret_cast(buffer_ + y_step * y + x_step * x); + } + + ScalarT *ptr(int y, int x) + { + return reinterpret_cast(buffer_ + y_step * y + x_step * x); + } + + unsigned char* buffer() + { + return buffer_; + } + + int sizeInBytes() const + { + return buffer_end_ - buffer_; + } +}; + +template +void flipHorizontal(const Mat &in, Mat& out) +{ + in.copyTo(out); + + typedef unsigned char type; + + int linestep = out.sizeInBytes() / out.height() / sizeof(type); + + type *first_line = reinterpret_cast(out.buffer()), *last_line = reinterpret_cast(out.buffer()) + (out.height() - 1) * linestep; + + + for(int y = 0; y < out.height() / 2; ++y) + { + for(int x = 0; x < linestep; ++x, ++first_line, ++last_line) + { + std::swap(*first_line, *last_line); + } + last_line -= 2 * linestep; + } +} + namespace libfreenect2 { @@ -64,7 +207,8 @@ inline int bfi(int width, int offset, int src2, int src3) class CpuDepthPacketProcessorImpl { public: - cv::Mat p0_table0, p0_table1, p0_table2, x_table, z_table; + Mat p0_table0, p0_table1, p0_table2; + Mat x_table, z_table; int16_t lut11to16[2048]; @@ -101,12 +245,12 @@ class CpuDepthPacketProcessorImpl void startTiming() { - timing_current_start = cv::getTickCount(); + //timing_current_start = cv::getTickCount(); } void stopTiming() { - timing_acc += (cv::getTickCount() - timing_current_start) / cv::getTickFrequency(); + //timing_acc += (cv::getTickCount() - timing_current_start) / cv::getTickFrequency(); timing_acc_n += 1.0; if(timing_acc_n >= 100.0) @@ -192,24 +336,27 @@ class CpuDepthPacketProcessorImpl return lut11to16[((i1 | i2) & 2047)]; } - void fill_trig_tables(cv::Mat& p0table, float trig_table[512*424][6]) + void fillTrigTable(Mat &p0table, float trig_table[512*424][6]) { - for (int i = 0; i < 512*424; i++) - { - float p0 = -((float)p0table.at(i)) * 0.000031 * M_PI; + int i = 0; + + for(int y = 0; y < 424; ++y) + for(int x = 0; x < 512; ++x, ++i) + { + float p0 = -((float)p0table.at(y, x)) * 0.000031 * M_PI; - float tmp0 = p0 + params.phase_in_rad[0]; - float tmp1 = p0 + params.phase_in_rad[1]; - float tmp2 = p0 + params.phase_in_rad[2]; + float tmp0 = p0 + params.phase_in_rad[0]; + float tmp1 = p0 + params.phase_in_rad[1]; + float tmp2 = p0 + params.phase_in_rad[2]; - trig_table[i][0] = std::cos(tmp0); - trig_table[i][1] = std::cos(tmp1); - trig_table[i][2] = std::cos(tmp2); + trig_table[i][0] = std::cos(tmp0); + trig_table[i][1] = std::cos(tmp1); + trig_table[i][2] = std::cos(tmp2); - trig_table[i][3] = std::sin(-tmp0); - trig_table[i][4] = std::sin(-tmp1); - trig_table[i][5] = std::sin(-tmp2); - } + trig_table[i][3] = std::sin(-tmp0); + trig_table[i][4] = std::sin(-tmp1); + trig_table[i][5] = std::sin(-tmp2); + } } void processMeasurementTriple(float trig_table[512*424][6], float abMultiplierPerFrq, int x, int y, const int32_t* m, float* m_out) @@ -223,7 +370,7 @@ class CpuDepthPacketProcessorImpl float sin_negtmp1 = trig_table[offset][4]; float sin_negtmp2 = trig_table[offset][5]; - float zmultiplier = z_table.at(y, x); + float zmultiplier = z_table.at(y, x); bool cond0 = 0 < zmultiplier; bool cond1 = (m[0] == 32767 || m[1] == 32767 || m[2] == 32767) && cond0; @@ -285,9 +432,9 @@ class CpuDepthPacketProcessorImpl processMeasurementTriple(trig_table2, params.ab_multiplier_per_frq[2], x, y, m2_raw, m2_out); } - void filterPixelStage1(int x, int y, const cv::Mat& m, float* m_out, bool& bilateral_max_edge_test) + void filterPixelStage1(int x, int y, const Mat >& m, float* m_out, bool& bilateral_max_edge_test) { - const float *m_ptr = m.ptr(y, x); + const float *m_ptr = (m.ptr(y, x)->val); bilateral_max_edge_test = true; if(x < 1 || y < 1 || x > 510 || y > 422) @@ -340,7 +487,7 @@ class CpuDepthPacketProcessorImpl continue; } - const float *other_m_ptr = m.ptr(y + yi, x + xi) + offset; + const float *other_m_ptr = (m.ptr(y + yi, x + xi)->val) + offset; float other_norm2 = other_m_ptr[0] * other_m_ptr[0] + other_m_ptr[1] * other_m_ptr[1]; // TODO: maybe fix numeric problems when norm = 0 - original code uses reciprocal square root, which returns +inf for +0 float other_inv_norm = 1.0f / std::sqrt(other_norm2); @@ -489,8 +636,8 @@ class CpuDepthPacketProcessorImpl } // this seems to be the phase to depth mapping :) - float zmultiplier = z_table.at(y, x); - float xmultiplier = x_table.at(y, x); + float zmultiplier = z_table.at(y, x); + float xmultiplier = x_table.at(y, x); phase = 0 < phase ? phase + params.phase_offset : phase; @@ -522,9 +669,9 @@ class CpuDepthPacketProcessorImpl //ir_out[2] = std::min(m2[2] * ab_output_multiplier, 65535.0f); } - void filterPixelStage2(int x, int y, cv::Mat &m, bool max_edge_test_ok, float *depth_out) + void filterPixelStage2(int x, int y, Mat > &m, bool max_edge_test_ok, float *depth_out) { - cv::Vec3f &depth_and_ir_sum = m.at(y, x); + Vec &depth_and_ir_sum = m.at(y, x); float &raw_depth = depth_and_ir_sum.val[0], &ir_sum = depth_and_ir_sum.val[2]; if(raw_depth >= params.min_depth && raw_depth <= params.max_depth) @@ -543,7 +690,7 @@ class CpuDepthPacketProcessorImpl { if(yi == 0 && xi == 0) continue; - cv::Vec3f &other = m.at(y + yi, x + xi); + Vec &other = m.at(y + yi, x + xi); ir_sum_acc += other.val[2]; squared_ir_sum_acc += other.val[2] * other.val[2]; @@ -636,68 +783,69 @@ void CpuDepthPacketProcessor::loadP0TablesFromCommandResponse(unsigned char* buf if(impl_->flip_ptables) { - cv::flip(cv::Mat(424, 512, CV_16UC1, p0table->p0table0), impl_->p0_table0, 0); - cv::flip(cv::Mat(424, 512, CV_16UC1, p0table->p0table1), impl_->p0_table1, 0); - cv::flip(cv::Mat(424, 512, CV_16UC1, p0table->p0table2), impl_->p0_table2, 0); - - impl_->fill_trig_tables(impl_->p0_table0, impl_->trig_table0); - impl_->fill_trig_tables(impl_->p0_table1, impl_->trig_table1); - impl_->fill_trig_tables(impl_->p0_table2, impl_->trig_table2); + flipHorizontal(Mat(424, 512, p0table->p0table0), impl_->p0_table0); + flipHorizontal(Mat(424, 512, p0table->p0table1), impl_->p0_table1); + flipHorizontal(Mat(424, 512, p0table->p0table2), impl_->p0_table2); } else { - cv::Mat(424, 512, CV_16UC1, p0table->p0table0).copyTo(impl_->p0_table0); - cv::Mat(424, 512, CV_16UC1, p0table->p0table1).copyTo(impl_->p0_table1); - cv::Mat(424, 512, CV_16UC1, p0table->p0table2).copyTo(impl_->p0_table2); + Mat p00(424, 512, p0table->p0table0); + p00.copyTo(impl_->p0_table0); + Mat(424, 512, p0table->p0table1).copyTo(impl_->p0_table1); + Mat(424, 512, p0table->p0table2).copyTo(impl_->p0_table2); } + + impl_->fillTrigTable(impl_->p0_table0, impl_->trig_table0); + impl_->fillTrigTable(impl_->p0_table1, impl_->trig_table1); + impl_->fillTrigTable(impl_->p0_table2, impl_->trig_table2); } void CpuDepthPacketProcessor::loadP0TablesFromFiles(const char* p0_filename, const char* p1_filename, const char* p2_filename) { - cv::Mat p0_table0(424, 512, CV_16UC1); - if(!loadBufferFromFile2(p0_filename, p0_table0.data, p0_table0.total() * p0_table0.elemSize())) + Mat p0_table0(424, 512); + if(!loadBufferFromFile2(p0_filename, p0_table0.buffer(), p0_table0.sizeInBytes())) { std::cerr << "[CpuDepthPacketProcessor::loadP0TablesFromFiles] Loading p0table 0 from '" << p0_filename << "' failed!" << std::endl; } - cv::Mat p0_table1(424, 512, CV_16UC1); - if(!loadBufferFromFile2(p1_filename, p0_table1.data, p0_table1.total() * p0_table1.elemSize())) + Mat p0_table1(424, 512); + if(!loadBufferFromFile2(p1_filename, p0_table1.buffer(), p0_table1.sizeInBytes())) { std::cerr << "[CpuDepthPacketProcessor::loadP0TablesFromFiles] Loading p0table 1 from '" << p1_filename << "' failed!" << std::endl; } - cv::Mat p0_table2(424, 512, CV_16UC1); - if(!loadBufferFromFile2(p2_filename, p0_table2.data, p0_table2.total() * p0_table2.elemSize())) + Mat p0_table2(424, 512); + if(!loadBufferFromFile2(p2_filename, p0_table2.buffer(), p0_table2.sizeInBytes())) { std::cerr << "[CpuDepthPacketProcessor::loadP0TablesFromFiles] Loading p0table 2 from '" << p2_filename << "' failed!" << std::endl; } if(impl_->flip_ptables) { - cv::flip(p0_table0, impl_->p0_table0, 0); - cv::flip(p0_table1, impl_->p0_table1, 0); - cv::flip(p0_table2, impl_->p0_table2, 0); + flipHorizontal(p0_table0, impl_->p0_table0); + flipHorizontal(p0_table1, impl_->p0_table1); + flipHorizontal(p0_table2, impl_->p0_table2); - impl_->fill_trig_tables(impl_->p0_table0, impl_->trig_table0); - impl_->fill_trig_tables(impl_->p0_table1, impl_->trig_table1); - impl_->fill_trig_tables(impl_->p0_table2, impl_->trig_table2); + impl_->fillTrigTable(impl_->p0_table0, impl_->trig_table0); + impl_->fillTrigTable(impl_->p0_table1, impl_->trig_table1); + impl_->fillTrigTable(impl_->p0_table2, impl_->trig_table2); } else { - impl_->fill_trig_tables(p0_table0, impl_->trig_table0); - impl_->fill_trig_tables(p0_table1, impl_->trig_table1); - impl_->fill_trig_tables(p0_table2, impl_->trig_table2); + impl_->fillTrigTable(p0_table0, impl_->trig_table0); + impl_->fillTrigTable(p0_table1, impl_->trig_table1); + impl_->fillTrigTable(p0_table2, impl_->trig_table2); } } void CpuDepthPacketProcessor::loadXTableFromFile(const char* filename) { - impl_->x_table.create(424, 512, CV_32FC1); + impl_->x_table.create(424, 512); const unsigned char *data; size_t length; if(loadResource("xTable.bin", &data, &length)) { - std::copy(data, data + length, impl_->x_table.data); + std::copy(data, data + length, impl_->x_table.buffer()); } else { @@ -707,14 +855,14 @@ void CpuDepthPacketProcessor::loadXTableFromFile(const char* filename) void CpuDepthPacketProcessor::loadZTableFromFile(const char* filename) { - impl_->z_table.create(424, 512, CV_32FC1); + impl_->z_table.create(424, 512); const unsigned char *data; size_t length; if(loadResource("zTable.bin", &data, &length)) { - std::copy(data, data + length, impl_->z_table.data); + std::copy(data, data + length, impl_->z_table.buffer()); } else { @@ -741,16 +889,20 @@ void CpuDepthPacketProcessor::process(const DepthPacket &packet) { if(listener_ == 0) return; - impl_->startTiming(); + //impl_->startTiming(); impl_->ir_frame->timestamp = packet.timestamp; impl_->depth_frame->timestamp = packet.timestamp; impl_->ir_frame->sequence = packet.sequence; impl_->depth_frame->sequence = packet.sequence; - cv::Mat m = cv::Mat::zeros(424, 512, CV_32FC(9)), m_filtered = cv::Mat::zeros(424, 512, CV_32FC(9)), m_max_edge_test = cv::Mat::ones(424, 512, CV_8UC1); + Mat > + m(424, 512), + m_filtered(424, 512) + ; + Mat m_max_edge_test(424, 512); - float *m_ptr = m.ptr(); + float *m_ptr = (m.ptr(0, 0)->val); for(int y = 0; y < 424; ++y) for(int x = 0; x < 512; ++x, m_ptr += 9) @@ -761,8 +913,8 @@ void CpuDepthPacketProcessor::process(const DepthPacket &packet) // bilateral filtering if(impl_->enable_bilateral_filter) { - float *m_filtered_ptr = m_filtered.ptr(); - unsigned char *m_max_edge_test_ptr = m_max_edge_test.ptr(); + float *m_filtered_ptr = (m_filtered.ptr(0, 0)->val); + unsigned char *m_max_edge_test_ptr = m_max_edge_test.ptr(0, 0); for(int y = 0; y < 424; ++y) for(int x = 0; x < 512; ++x, m_filtered_ptr += 9, ++m_max_edge_test_ptr) @@ -772,39 +924,39 @@ void CpuDepthPacketProcessor::process(const DepthPacket &packet) *m_max_edge_test_ptr = max_edge_test_val ? 1 : 0; } - m_ptr = m_filtered.ptr(); + m_ptr = (m_filtered.ptr(0, 0)->val); } else { - m_ptr = m.ptr(); + m_ptr = (m.ptr(0, 0)->val); } - cv::Mat out_ir(424, 512, CV_32FC1, impl_->ir_frame->data), out_depth(424, 512, CV_32FC1, impl_->depth_frame->data); + Mat out_ir(424, 512, impl_->ir_frame->data), out_depth(424, 512, impl_->depth_frame->data); if(impl_->enable_edge_filter) { - cv::Mat depth_ir_sum(424, 512, CV_32FC3); - cv::Vec3f *depth_ir_sum_ptr = depth_ir_sum.ptr(); - unsigned char *m_max_edge_test_ptr = m_max_edge_test.ptr(); + Mat > depth_ir_sum(424, 512); + Vec *depth_ir_sum_ptr = depth_ir_sum.ptr(0, 0); + unsigned char *m_max_edge_test_ptr = m_max_edge_test.ptr(0, 0); for(int y = 0; y < 424; ++y) for(int x = 0; x < 512; ++x, m_ptr += 9, ++m_max_edge_test_ptr, ++depth_ir_sum_ptr) { float raw_depth, ir_sum; - impl_->processPixelStage2(x, y, m_ptr + 0, m_ptr + 3, m_ptr + 6, out_ir.ptr(423 - y, x), &raw_depth, &ir_sum); + impl_->processPixelStage2(x, y, m_ptr + 0, m_ptr + 3, m_ptr + 6, out_ir.ptr(423 - y, x), &raw_depth, &ir_sum); depth_ir_sum_ptr->val[0] = raw_depth; depth_ir_sum_ptr->val[1] = *m_max_edge_test_ptr == 1 ? raw_depth : 0; depth_ir_sum_ptr->val[2] = ir_sum; } - m_max_edge_test_ptr = m_max_edge_test.ptr(); + m_max_edge_test_ptr = m_max_edge_test.ptr(0, 0); for(int y = 0; y < 424; ++y) for(int x = 0; x < 512; ++x, ++m_max_edge_test_ptr) { - impl_->filterPixelStage2(x, y, depth_ir_sum, *m_max_edge_test_ptr == 1, out_depth.ptr(423 - y, x)); + impl_->filterPixelStage2(x, y, depth_ir_sum, *m_max_edge_test_ptr == 1, out_depth.ptr(423 - y, x)); } } else @@ -812,7 +964,7 @@ void CpuDepthPacketProcessor::process(const DepthPacket &packet) for(int y = 0; y < 424; ++y) for(int x = 0; x < 512; ++x, m_ptr += 9) { - impl_->processPixelStage2(x, y, m_ptr + 0, m_ptr + 3, m_ptr + 6, out_ir.ptr(423 - y, x), out_depth.ptr(423 - y, x), 0); + impl_->processPixelStage2(x, y, m_ptr + 0, m_ptr + 3, m_ptr + 6, out_ir.ptr(423 - y, x), out_depth.ptr(423 - y, x), 0); } } @@ -826,7 +978,7 @@ void CpuDepthPacketProcessor::process(const DepthPacket &packet) impl_->newDepthFrame(); } - impl_->stopTiming(); + //impl_->stopTiming(); } } /* namespace libfreenect2 */ diff --git a/examples/protonect/src/opencl_depth_packet_processor.cpp b/examples/protonect/src/opencl_depth_packet_processor.cpp index cc13587ab..f4aa68f29 100644 --- a/examples/protonect/src/opencl_depth_packet_processor.cpp +++ b/examples/protonect/src/opencl_depth_packet_processor.cpp @@ -28,7 +28,7 @@ #include #include -#include +//#include #include #include #include @@ -520,12 +520,12 @@ class OpenCLDepthPacketProcessorImpl void startTiming() { - timing_current_start = cv::getTickCount(); + //timing_current_start = cv::getTickCount(); } void stopTiming() { - timing_acc += (cv::getTickCount() - timing_current_start) / cv::getTickFrequency(); + //timing_acc += (cv::getTickCount() - timing_current_start) / cv::getTickFrequency(); timing_acc_n += 1.0; if(timing_acc_n >= 100.0) diff --git a/examples/protonect/src/turbo_jpeg_rgb_packet_processor.cpp b/examples/protonect/src/turbo_jpeg_rgb_packet_processor.cpp index c93bec9c7..137f0dc80 100644 --- a/examples/protonect/src/turbo_jpeg_rgb_packet_processor.cpp +++ b/examples/protonect/src/turbo_jpeg_rgb_packet_processor.cpp @@ -25,9 +25,11 @@ */ #include - -#include +#ifdef LIBFREENECT2_OPENCV_FOUND + #include +#endif #include +#include namespace libfreenect2 { @@ -78,11 +80,14 @@ class TurboJpegRgbPacketProcessorImpl void startTiming() { +#ifdef LIBFREENECT2_OPENCV_FOUND timing_current_start = cv::getTickCount(); +#endif } void stopTiming() { +#ifdef LIBFREENECT2_OPENCV_FOUND timing_acc += (cv::getTickCount() - timing_current_start) / cv::getTickFrequency(); timing_acc_n += 1.0; @@ -93,6 +98,7 @@ class TurboJpegRgbPacketProcessorImpl timing_acc = 0.0; timing_acc_n = 0.0; } +#endif } }; @@ -110,7 +116,7 @@ void TurboJpegRgbPacketProcessor::process(const RgbPacket &packet) { if(impl_->decompressor != 0 && listener_ != 0) { - impl_->startTiming(); + //impl_->startTiming(); impl_->frame->timestamp = packet.timestamp; impl_->frame->sequence = packet.sequence; @@ -129,7 +135,7 @@ void TurboJpegRgbPacketProcessor::process(const RgbPacket &packet) std::cerr << "[TurboJpegRgbPacketProcessor::doProcess] Failed to decompress rgb image! TurboJPEG error: '" << tjGetErrorStr() << "'" << std::endl; } - impl_->stopTiming(); + //impl_->stopTiming(); } } diff --git a/examples/protonect/viewer.cpp b/examples/protonect/viewer.cpp new file mode 100644 index 000000000..bd1958053 --- /dev/null +++ b/examples/protonect/viewer.cpp @@ -0,0 +1,193 @@ +#include "viewer.h" + +Viewer::Viewer() : shader_folder("src/shader/") +{ + // init glfw - if already initialized nothing happens + int init = glfwInit(); + + // setup context + glfwDefaultWindowHints(); + glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); + glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3); +#ifdef __APPLE__ + glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE); +#endif + glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); + //glfwWindowHint(GLFW_VISIBLE, debug ? GL_TRUE : GL_FALSE); + + window = glfwCreateWindow(1280, 800, "Viewer", 0, NULL); +} + +void Viewer::initialize() +{ + glfwMakeContextCurrent(window); + OpenGLBindings *b = new OpenGLBindings(); + flextInit(window, b); + gl(b); + + std::string vertexshadersrc = "" + "#version 330\n" + + "in vec2 Position;" + "in vec2 TexCoord;" + + "out VertexData{" + "vec2 TexCoord;" + "} VertexOut;" + + "void main(void)" + "{" + " gl_Position = vec4(Position, 0.0, 1.0);" + " VertexOut.TexCoord = TexCoord;" + "}"; + std::string grayfragmentshader = "" + "#version 330\n" + + "uniform sampler2DRect Data;" + + "vec4 tempColor;" + "in VertexData{" + " vec2 TexCoord;" + "} FragmentIn;" + + "layout(location = 0) out vec4 Color;" + + "void main(void)" + "{" + "ivec2 uv = ivec2(FragmentIn.TexCoord.x, FragmentIn.TexCoord.y);" + "tempColor = texelFetch(Data, uv);" + "Color = vec4(tempColor.x, tempColor.x, tempColor.x, 1);" + "}"; + std::string fragmentshader = "" + "#version 330\n" + + "uniform sampler2DRect Data;" + + "in VertexData{" + " vec2 TexCoord;" + "} FragmentIn;" + + "layout(location = 0) out vec4 Color;" + + "void main(void)" + "{" + " ivec2 uv = ivec2(FragmentIn.TexCoord.x, FragmentIn.TexCoord.y);" + + " Color = texelFetch(Data, uv);" + "}"; + + renderShader.setVertexShader(vertexshadersrc); + renderShader.setFragmentShader(fragmentshader); + renderShader.build(); + + renderGrayShader.setVertexShader(vertexshadersrc); + renderGrayShader.setFragmentShader(grayfragmentshader); + renderGrayShader.build(); + + + glfwSetWindowUserPointer(window, this); + glfwSetKeyCallback(window, Viewer::key_callbackstatic); + + shouldStop = false; +} + +void Viewer::key_callbackstatic(GLFWwindow* window, int key, int scancode, int action, int mods) +{ + Viewer* viewer = reinterpret_cast(glfwGetWindowUserPointer(window)); + viewer->key_callback(window, key, scancode, action, mods); +} + +void Viewer::key_callback(GLFWwindow* window, int key, int scancode, int action, int mods) +{ + if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS) + shouldStop = true; +} + +void Viewer::onOpenGLBindingsChanged(OpenGLBindings *b) +{ + renderShader.gl(b); + renderGrayShader.gl(b); + rgb.gl(b); + ir.gl(b); +} + +bool Viewer::render() +{ + // wipe the drawing surface clear + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + GLint x = 0, y = 0, width = 600, height = 400; + + std::map::iterator iter; + + for (iter = frames.begin(); iter != frames.end(); ++iter) + { + libfreenect2::Frame* frame = iter->second; + + glViewport(x, y, width, height); + x += width; + if (x >= 1024) + { + x = 0; + y += height; + } + + Vertex bl = { -1.0f, -1.0f, 0.0f, 0.0f }, br = { 1.0f, -1.0f, frame->width, 0.0f }, tl = { -1.0f, 1.0f, 0.0f, frame->height }, tr = { 1.0f, 1.0f, frame->width, frame->height }; + Vertex vertices[] = { + bl, tl, tr, tr, br, bl + }; + + gl()->glGenBuffers(1, &triangle_vbo); + gl()->glGenVertexArrays(1, &triangle_vao); + + gl()->glBindVertexArray(triangle_vao); + gl()->glBindBuffer(GL_ARRAY_BUFFER, triangle_vbo); + gl()->glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW); + + GLint position_attr = renderShader.getAttributeLocation("Position"); + gl()->glVertexAttribPointer(position_attr, 2, GL_FLOAT, GL_FALSE, sizeof(Vertex), (GLvoid*)0); + gl()->glEnableVertexAttribArray(position_attr); + + GLint texcoord_attr = renderShader.getAttributeLocation("TexCoord"); + gl()->glVertexAttribPointer(texcoord_attr, 2, GL_FLOAT, GL_FALSE, sizeof(Vertex), (GLvoid*)(2 * sizeof(float))); + gl()->glEnableVertexAttribArray(texcoord_attr); + + + if (iter->first == "RGB" || iter->first == "registered") + { + renderShader.use(); + + rgb.allocate(frame->width, frame->height); + std::copy(frame->data, frame->data + frame->width * frame->height * frame->bytes_per_pixel, rgb.data); + rgb.flipY(); + rgb.upload(); + glDrawArrays(GL_TRIANGLES, 0, 6); + + rgb.deallocate(); + + } + else + { + renderGrayShader.use(); + + ir.allocate(frame->width, frame->height); + std::copy(frame->data, frame->data + frame->width * frame->height * frame->bytes_per_pixel, ir.data); + ir.flipY(); + ir.upload(); + glDrawArrays(GL_TRIANGLES, 0, 6); + ir.deallocate(); + } + } + + // put the stuff we've been drawing onto the display + glfwSwapBuffers(window); + // update other events like input handling + glfwPollEvents(); + + return shouldStop || glfwWindowShouldClose(window); +} + +void Viewer::addFrame(std::string id, libfreenect2::Frame* frame) +{ + frames[id] = frame; +} \ No newline at end of file diff --git a/examples/protonect/viewer.h b/examples/protonect/viewer.h new file mode 100644 index 000000000..306ee6a36 --- /dev/null +++ b/examples/protonect/viewer.h @@ -0,0 +1,288 @@ +#ifndef VIEWER_H +#define VIEWER_H + +#include + +#include +#include +#include +#include + +#include <../src/flextGL.h> +#include + +struct Vertex +{ + float x, y; + float u, v; +}; + +class WithOpenGLBindings +{ +private: + OpenGLBindings *bindings; +protected: + WithOpenGLBindings() : bindings(0) {} + virtual ~WithOpenGLBindings() {} + + virtual void onOpenGLBindingsChanged(OpenGLBindings *b) { } +public: + void gl(OpenGLBindings *bindings) + { + this->bindings = bindings; + onOpenGLBindingsChanged(this->bindings); + } + + OpenGLBindings *gl() + { + return bindings; + } +}; + +template +struct ImageFormat +{ + static const size_t BytesPerPixel = TBytesPerPixel; + static const GLenum InternalFormat = TInternalFormat; + static const GLenum Format = TFormat; + static const GLenum Type = TType; +}; + +typedef ImageFormat<1, GL_R8UI, GL_RED_INTEGER, GL_UNSIGNED_BYTE> U8C1; +typedef ImageFormat<2, GL_R16I, GL_RED_INTEGER, GL_SHORT> S16C1; +typedef ImageFormat<2, GL_R16UI, GL_RED_INTEGER, GL_UNSIGNED_SHORT> U16C1; +typedef ImageFormat<4, GL_R32F, GL_RED, GL_FLOAT> F32C1; +typedef ImageFormat<8, GL_RG32F, GL_RG, GL_FLOAT> F32C2; +typedef ImageFormat<12, GL_RGB32F, GL_RGB, GL_FLOAT> F32C3; +typedef ImageFormat<4, GL_RGBA, GL_BGRA, GL_UNSIGNED_BYTE> F8C4; +typedef ImageFormat<16, GL_RGBA32F, GL_RGBA, GL_FLOAT> F32C4; + +template +struct Texture : public WithOpenGLBindings +{ +protected: + size_t bytes_per_pixel, height, width; + +public: + GLuint texture; + unsigned char *data; + size_t size; + + Texture() : texture(0), data(0), size(0), bytes_per_pixel(FormatT::BytesPerPixel), height(0), width(0) + { + } + + void bindToUnit(GLenum unit) + { + gl()->glActiveTexture(unit); + glBindTexture(GL_TEXTURE_RECTANGLE, texture); + } + + void allocate(size_t new_width, size_t new_height) + { + width = new_width; + height = new_height; + size = height * width * bytes_per_pixel; + data = new unsigned char[size]; + + glGenTextures(1, &texture); + bindToUnit(GL_TEXTURE0); + glTexParameteri(GL_TEXTURE_RECTANGLE, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER); + glTexParameteri(GL_TEXTURE_RECTANGLE, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_BORDER); + glTexParameteri(GL_TEXTURE_RECTANGLE, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_RECTANGLE, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glTexImage2D(GL_TEXTURE_RECTANGLE, 0, FormatT::InternalFormat, width, height, 0, FormatT::Format, FormatT::Type, 0); + } + + void deallocate() + { + glDeleteTextures(1, &texture); + delete[] data; + } + + void upload() + { + bindToUnit(GL_TEXTURE0); + glPixelStorei(GL_UNPACK_ALIGNMENT, 1); + glTexSubImage2D(GL_TEXTURE_RECTANGLE, /*level*/0, /*xoffset*/0, /*yoffset*/0, width, height, FormatT::Format, FormatT::Type, data); + } + + void download() + { + downloadToBuffer(data); + } + + void downloadToBuffer(unsigned char *data) + { + glReadPixels(0, 0, width, height, FormatT::Format, FormatT::Type, data); + } + + void flipY() + { + flipYBuffer(data); + } + + void flipYBuffer(unsigned char *data) + { + typedef unsigned char type; + + int linestep = width * bytes_per_pixel / sizeof(type); + + type *first_line = reinterpret_cast(data), *last_line = reinterpret_cast(data) + (height - 1) * linestep; + + for (int y = 0; y < height / 2; ++y) + { + for (int x = 0; x < linestep; ++x, ++first_line, ++last_line) + { + std::swap(*first_line, *last_line); + } + last_line -= 2 * linestep; + } + } + + libfreenect2::Frame *downloadToNewFrame() + { + libfreenect2::Frame *f = new libfreenect2::Frame(width, height, bytes_per_pixel); + downloadToBuffer(f->data); + flipYBuffer(f->data); + + return f; + } +}; + +struct ShaderProgram : public WithOpenGLBindings +{ + GLuint program, vertex_shader, fragment_shader; + + char error_buffer[2048]; + + ShaderProgram() : + program(0), + vertex_shader(0), + fragment_shader(0) + { + } + + void setVertexShader(const std::string& src) + { + const char* src_ = src.c_str(); + int length_ = src.length(); + vertex_shader = gl()->glCreateShader(GL_VERTEX_SHADER); + gl()->glShaderSource(vertex_shader, 1, &src_, &length_); + } + + void setFragmentShader(const std::string& src) + { + const char* src_ = src.c_str(); + int length_ = src.length(); + fragment_shader = gl()->glCreateShader(GL_FRAGMENT_SHADER); + gl()->glShaderSource(fragment_shader, 1, &src_, &length_); + } + + void build() + { + GLint status; + + gl()->glCompileShader(vertex_shader); + gl()->glGetShaderiv(vertex_shader, GL_COMPILE_STATUS, &status); + + if (status != GL_TRUE) + { + gl()->glGetShaderInfoLog(vertex_shader, sizeof(error_buffer), NULL, error_buffer); + + std::cerr << "[ShaderProgram::build] failed to compile vertex shader!" << std::endl; + std::cerr << error_buffer << std::endl; + } + + gl()->glCompileShader(fragment_shader); + + gl()->glGetShaderiv(fragment_shader, GL_COMPILE_STATUS, &status); + if (status != GL_TRUE) + { + gl()->glGetShaderInfoLog(fragment_shader, sizeof(error_buffer), NULL, error_buffer); + + std::cerr << "[ShaderProgram::build] failed to compile fragment shader!" << std::endl; + std::cerr << error_buffer << std::endl; + } + + program = gl()->glCreateProgram(); + gl()->glAttachShader(program, vertex_shader); + gl()->glAttachShader(program, fragment_shader); + + gl()->glLinkProgram(program); + + gl()->glGetProgramiv(program, GL_LINK_STATUS, &status); + + if (status != GL_TRUE) + { + gl()->glGetProgramInfoLog(program, sizeof(error_buffer), NULL, error_buffer); + std::cerr << "[ShaderProgram::build] failed to link shader program!" << std::endl; + std::cerr << error_buffer << std::endl; + } + } + + GLint getAttributeLocation(const std::string& name) + { + return gl()->glGetAttribLocation(program, name.c_str()); + } + + void setUniform(const std::string& name, GLint value) + { + GLint idx = gl()->glGetUniformLocation(program, name.c_str()); + if (idx == -1) return; + + gl()->glUniform1i(idx, value); + } + + void setUniform(const std::string& name, GLfloat value) + { + GLint idx = gl()->glGetUniformLocation(program, name.c_str()); + if (idx == -1) return; + + gl()->glUniform1f(idx, value); + } + + void setUniformVector3(const std::string& name, GLfloat value[3]) + { + GLint idx = gl()->glGetUniformLocation(program, name.c_str()); + if (idx == -1) return; + + gl()->glUniform3fv(idx, 1, value); + } + + void setUniformMatrix3(const std::string& name, GLfloat value[9]) + { + GLint idx = gl()->glGetUniformLocation(program, name.c_str()); + if (idx == -1) return; + + gl()->glUniformMatrix3fv(idx, 1, false, value); + } + + void use() + { + gl()->glUseProgram(program); + } +}; + +class Viewer : WithOpenGLBindings { +private: + bool shouldStop; + GLFWwindow* window; + GLuint triangle_vbo, triangle_vao; + ShaderProgram renderShader; + ShaderProgram renderGrayShader; + std::string shader_folder; + std::map frames; + Texture rgb; + Texture ir; +public: + Viewer(); + void initialize(); + virtual void onOpenGLBindingsChanged(OpenGLBindings *b); + bool render(); + void addFrame(std::string id,libfreenect2::Frame* frame); + void key_callback(GLFWwindow* window, int key, int scancode, int action, int mods); + static void key_callbackstatic(GLFWwindow* window, int key, int scancode, int action, int mods); +}; + +#endif \ No newline at end of file