├── .gitattributes ├── .gitignore ├── CMakeLists.txt ├── README.md ├── cmake └── Modules │ └── FindRSSDK.cmake ├── images └── screenshot.png ├── include ├── buffers.h ├── impl │ └── buffers.hpp ├── io_exception.h ├── real_sense │ ├── real_sense_device_manager.h │ └── time.h └── real_sense_grabber.h └── src ├── io_exception.cpp ├── real_sense └── real_sense_device_manager.cpp ├── real_sense_grabber.cpp ├── real_sense_viewer.cpp └── tutorial.cpp /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | *.sln merge=union 7 | *.csproj merge=union 8 | *.vbproj merge=union 9 | *.fsproj merge=union 10 | *.dbproj merge=union 11 | 12 | # Standard to msysgit 13 | *.doc diff=astextplain 14 | *.DOC diff=astextplain 15 | *.docx diff=astextplain 16 | *.DOCX diff=astextplain 17 | *.dot diff=astextplain 18 | *.DOT diff=astextplain 19 | *.pdf diff=astextplain 20 | *.PDF diff=astextplain 21 | *.rtf diff=astextplain 22 | *.RTF diff=astextplain 23 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /build 2 | 3 | # Windows image file caches 4 | Thumbs.db 5 | ehthumbs.db 6 | 7 | # Folder config file 8 | Desktop.ini 9 | 10 | # Recycle Bin used on file shares 11 | $RECYCLE.BIN/ 12 | 13 | # Windows Installer files 14 | *.cab 15 | *.msi 16 | *.msm 17 | *.msp 18 | 19 | # ========================= 20 | # Operating System Files 21 | # ========================= 22 | 23 | # OSX 24 | # ========================= 25 | 26 | .DS_Store 27 | .AppleDouble 28 | .LSOverride 29 | 30 | # Icon must end with two \r 31 | Icon 32 | 33 | # Thumbnails 34 | ._* 35 | 36 | # Files that might appear on external disk 37 | .Spotlight-V100 38 | .Trashes 39 | 40 | # Directories potentially created on remote AFP share 41 | .AppleDB 42 | .AppleDesktop 43 | Network Trash Folder 44 | Temporary Items 45 | .apdisk 46 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR) 2 | 3 | project(rs) 4 | 5 | set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake/Modules/" ${CMAKE_MODULE_PATH}) 6 | set_property(GLOBAL PROPERTY USE_FOLDERS ON) 7 | 8 | find_package(PCL 1.7.2 REQUIRED) 9 | find_package(RSSDK REQUIRED) 10 | 11 | include_directories( 12 | include 13 | ${PCL_INCLUDE_DIRS} 14 | ${RSSDK_INCLUDE_DIRS} 15 | ) 16 | 17 | link_directories( 18 | ${PCL_LIBRARY_DIRS} 19 | ) 20 | 21 | add_definitions( 22 | ${PCL_DEFINITIONS} 23 | ) 24 | 25 | add_library(real_sense 26 | src/real_sense/real_sense_device_manager.cpp 27 | src/real_sense_grabber.cpp 28 | src/io_exception.cpp 29 | include/io_exception.h 30 | include/real_sense_grabber.h 31 | include/buffers.h 32 | include/impl/buffers.hpp 33 | include/real_sense/real_sense_device_manager.h 34 | include/real_sense/time.h 35 | ) 36 | 37 | add_executable(real_sense_viewer 38 | src/real_sense_viewer.cpp 39 | ) 40 | target_link_libraries(real_sense_viewer 41 | real_sense 42 | ${PCL_LIBRARIES} 43 | ${RSSDK_LIBRARIES} 44 | ) 45 | 46 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Requirements 2 | ============ 3 | 4 | 1. Point Cloud Library 1.7.2 5 | 6 | 2. [Intel® RealSense™ SDK for Windows](https://software.intel.com/en-us/intel-realsense-sdk/download) 7 | 8 | Installation 9 | ============ 10 | 11 | 1. Clone this repository: 12 | 13 | ```bash 14 | git clone https://github.com/taketwo/rs.git rs 15 | cd rs 16 | ``` 17 | 18 | 2. Configure the project: 19 | 20 | ```bash 21 | mkdir build 22 | cd build 23 | cmake -DBUILD_TYPE=Release .. 24 | ``` 25 | 26 | Note: configuration process might fail if some of the required packages are 27 | not found. In this case it is convenient to use `cmake-gui` to manually 28 | specify locations. 29 | 30 | ```bash 31 | cmake-gui .. 32 | ``` 33 | 34 | Press "Configure" and "Generate" after the locations have been specified. 35 | 36 | 3. Open the "rs.sln" solution file created in the previous step with Visual 37 | Studio. Press F7 to build the project. 38 | 39 | Real Sense Viewer 40 | ================= 41 | 42 | Connect a RealSense camera and run `real_sense_viewer.exe`. 43 | 44 | ![Real Sense Viewer](images/screenshot.png) 45 | 46 | Run with `--help` option to see the usage guide: 47 | 48 | ``` 49 | **************************************************************************** 50 | * * 51 | * REAL SENSE VIEWER - Usage Guide * 52 | * * 53 | **************************************************************************** 54 | 55 | Usage: real_sense_viewer.exe [Options] device_id 56 | 57 | Options: 58 | 59 | --help, -h : Show this help 60 | --list, -l : List connected RealSense devices and supported modes 61 | --mode : Use capture mode from the list of supported modes 62 | 63 | Keyboard commands: 64 | 65 | When the focus is on the viewer window, the following keyboard commands 66 | are available: 67 | * t/T : increase or decrease depth data confidence threshold 68 | * k : enable next temporal filtering method 69 | * b : toggle bilateral filtering 70 | * a/A : increase or decrease bilateral filter spatial sigma 71 | * z/Z : increase or decrease bilateral filter range sigma 72 | * s : save the last grabbed cloud to disk 73 | * h : print the list of standard PCL viewer commands 74 | 75 | Notes: 76 | 77 | The device to grab data from is selected using device_id argument. It 78 | could be either: 79 | * serial number (e.g. 231400041-03) 80 | * device index (e.g. #2 for the second connected device) 81 | 82 | If device_id is not given, then the first available device will be used. 83 | 84 | If capture mode is not given, then the grabber will try to enable both 85 | depth and color streams at VGA resolution and 30 Hz framerate. If this 86 | particular mode is not available, the one that most closely matches this 87 | specification will be chosen. 88 | ``` -------------------------------------------------------------------------------- /cmake/Modules/FindRSSDK.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # Find Intel RealSense SDK 3 | # 4 | # find_package(RSSDK) 5 | # 6 | # Variables defined by this module: 7 | # 8 | # RSSDK_FOUND True if RealSense SDK was found 9 | # RSSDK_VERSION The version of RealSense SDK 10 | # RSSDK_INCLUDE_DIRS The location(s) of RealSense SDK headers 11 | # RSSDK_LIBRARIES Libraries needed to use RealSense SDK 12 | 13 | find_path(RSSDK_DIR include/pxcversion.h 14 | PATHS "$ENV{RSSDK_DIR}" 15 | "$ENV{PROGRAMFILES}/Intel/RSSDK" 16 | "$ENV{PROGRAMW6432}/Intel/RSSDK" 17 | "C:/Program Files (x86)/Intel/RSSDK" 18 | "C:/Program Files/Intel/RSSDK" 19 | DOC "RealSense SDK directory") 20 | 21 | if(RSSDK_DIR) 22 | 23 | # Include directories 24 | set(RSSDK_INCLUDE_DIRS ${RSSDK_DIR}/include) 25 | mark_as_advanced(RSSDK_INCLUDE_DIRS) 26 | 27 | # Libraries 28 | set(RSSDK_RELEASE_NAME libpxc.lib) 29 | set(RSSDK_DEBUG_NAME libpxc_d.lib) 30 | set(RSSDK_SUFFIX Win32) 31 | if(CMAKE_SIZEOF_VOID_P EQUAL 8) 32 | set(RSSDK_SUFFIX x64) 33 | endif() 34 | find_library(RSSDK_LIBRARY 35 | NAMES ${RSSDK_RELEASE_NAME} 36 | PATHS "${RSSDK_DIR}/lib/" NO_DEFAULT_PATH 37 | PATH_SUFFIXES ${RSSDK_SUFFIX}) 38 | find_library(RSSDK_LIBRARY_DEBUG 39 | NAMES ${RSSDK_DEBUG_NAME} ${RSSDK_RELEASE_NAME} 40 | PATHS "${RSSDK_DIR}/lib/" NO_DEFAULT_PATH 41 | PATH_SUFFIXES ${RSSDK_SUFFIX}) 42 | if(NOT RSSDK_LIBRARY_DEBUG) 43 | set(RSSDK_LIBRARY_DEBUG ${RSSDK_LIBRARY}) 44 | endif() 45 | set(RSSDK_LIBRARIES optimized ${RSSDK_LIBRARY} debug ${RSSDK_LIBRARY_DEBUG}) 46 | mark_as_advanced(RSSDK_LIBRARY RSSDK_LIBRARY_DEBUG) 47 | 48 | # Version 49 | set(RSSDK_VERSION 0) 50 | file(STRINGS "${RSSDK_INCLUDE_DIRS}/pxcversion.h" _pxcversion_H_CONTENTS REGEX "#define PXC_VERSION_.*") 51 | set(_RSSDK_VERSION_REGEX "([0-9]+)") 52 | foreach(v MAJOR MINOR BUILD REVISION) 53 | if("${_pxcversion_H_CONTENTS}" MATCHES ".*#define PXC_VERSION_${v} *${_RSSDK_VERSION_REGEX}.*") 54 | set(RSSDK_VERSION_${v} "${CMAKE_MATCH_1}") 55 | endif() 56 | endforeach() 57 | unset(_pxcversion_H_CONTENTS) 58 | set(RSSDK_VERSION "${RSSDK_VERSION_MAJOR}.${RSSDK_VERSION_MINOR}.${RSSDK_VERSION_BUILD}.${RSSDK_VERSION_REVISION}") 59 | 60 | endif() 61 | 62 | include(FindPackageHandleStandardArgs) 63 | find_package_handle_standard_args(RSSDK 64 | FOUND_VAR RSSDK_FOUND 65 | REQUIRED_VARS RSSDK_LIBRARIES RSSDK_INCLUDE_DIRS 66 | VERSION_VAR RSSDK_VERSION 67 | ) 68 | 69 | if(MSVC) 70 | set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} /NODEFAULTLIB:LIBCMTD") 71 | endif() 72 | -------------------------------------------------------------------------------- /images/screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/taketwo/rs/d5c225dad701a57c8e54bfd1c6c2d22e22e03a8b/images/screenshot.png -------------------------------------------------------------------------------- /include/buffers.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2014-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_IO_BUFFERS_H 39 | #define PCL_IO_BUFFERS_H 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | namespace pcl 48 | { 49 | 50 | namespace io 51 | { 52 | 53 | template 54 | class Buffer 55 | { 56 | 57 | public: 58 | 59 | typedef T value_type; 60 | 61 | virtual 62 | ~Buffer (); 63 | 64 | virtual T 65 | operator[] (size_t idx) const = 0; 66 | 67 | virtual void 68 | push (std::vector& data) = 0; 69 | 70 | inline size_t 71 | size () const 72 | { 73 | return (size_); 74 | } 75 | 76 | protected: 77 | 78 | Buffer (size_t size); 79 | 80 | const size_t size_; 81 | 82 | }; 83 | 84 | template 85 | class SingleBuffer : public Buffer 86 | { 87 | 88 | public: 89 | 90 | SingleBuffer (size_t size); 91 | 92 | virtual 93 | ~SingleBuffer (); 94 | 95 | virtual T 96 | operator[] (size_t idx) const; 97 | 98 | virtual void 99 | push (std::vector& data); 100 | 101 | private: 102 | 103 | std::vector data_; 104 | 105 | using Buffer::size_; 106 | 107 | }; 108 | 109 | template 110 | class MedianBuffer : public Buffer 111 | { 112 | 113 | public: 114 | 115 | MedianBuffer (size_t size, size_t window_size); 116 | 117 | virtual 118 | ~MedianBuffer (); 119 | 120 | virtual T 121 | operator[] (size_t idx) const; 122 | 123 | virtual void 124 | push (std::vector& data); 125 | 126 | private: 127 | 128 | /** Compare two data elements. 129 | * 130 | * Invalid value is assumed to be larger than everything else. If both values 131 | * are invalid, they are assumed to be equal. 132 | * 133 | * \return -1 if \c a < \c b, 0 if \c a == \c b, 1 if \c a > \c b */ 134 | static int compare (T a, T b); 135 | 136 | const size_t window_size_; 137 | const size_t midpoint_; 138 | 139 | /// Data pushed into the buffer (last window_size_ chunks), logically 140 | /// organized as a circular buffer 141 | std::vector > data_; 142 | 143 | /// Index of the last pushed data chunk in the data_ circular buffer 144 | size_t data_current_idx_; 145 | 146 | /// Indices that the argsort function would produce for data_ (with 147 | /// dimensions swapped) 148 | std::vector > data_argsort_indices_; 149 | 150 | /// Number of invalid values in the buffer 151 | std::vector data_invalid_count_; 152 | 153 | using Buffer::size_; 154 | 155 | }; 156 | 157 | template 158 | class AverageBuffer : public Buffer 159 | { 160 | 161 | public: 162 | 163 | AverageBuffer (size_t size, size_t window_size); 164 | 165 | virtual 166 | ~AverageBuffer (); 167 | 168 | virtual T 169 | operator[] (size_t idx) const; 170 | 171 | virtual void 172 | push (std::vector& data); 173 | 174 | private: 175 | 176 | const size_t window_size_; 177 | 178 | /// Data pushed into the buffer (last window_size_ chunks), logically 179 | /// organized as a circular buffer 180 | std::vector > data_; 181 | 182 | /// Index of the last pushed data chunk in the data_ circular buffer 183 | size_t data_current_idx_; 184 | 185 | /// Current sum of the buffer 186 | std::vector data_sum_; 187 | 188 | /// Number of invalid values in the buffer 189 | std::vector data_invalid_count_; 190 | 191 | using Buffer::size_; 192 | 193 | }; 194 | 195 | } 196 | 197 | } 198 | 199 | #include "impl/buffers.hpp" 200 | 201 | #endif /* PCL_IO_BUFFERS_H */ 202 | 203 | -------------------------------------------------------------------------------- /include/impl/buffers.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2014-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include "buffers.h" 44 | 45 | template 46 | struct buffer_traits 47 | { 48 | static T invalid () { return 0; } 49 | static bool is_invalid (T value) { return value == invalid (); }; 50 | }; 51 | 52 | template <> 53 | struct buffer_traits 54 | { 55 | static float invalid () { return std::numeric_limits::quiet_NaN (); }; 56 | static bool is_invalid (float value) { return pcl_isnan (value); }; 57 | }; 58 | 59 | template 60 | pcl::io::Buffer::Buffer (size_t size) 61 | : size_ (size) 62 | { 63 | } 64 | 65 | template 66 | pcl::io::Buffer::~Buffer () 67 | { 68 | } 69 | 70 | template 71 | pcl::io::SingleBuffer::SingleBuffer (size_t size) 72 | : Buffer (size) 73 | , data_ (size, buffer_traits::invalid ()) 74 | { 75 | } 76 | 77 | template 78 | pcl::io::SingleBuffer::~SingleBuffer () 79 | { 80 | } 81 | 82 | template T 83 | pcl::io::SingleBuffer::operator[] (size_t idx) const 84 | { 85 | assert (idx < size_); 86 | return (data_[idx]); 87 | } 88 | 89 | template void 90 | pcl::io::SingleBuffer::push (std::vector& data) 91 | { 92 | assert (data.size () == size_); 93 | data_.swap (data); 94 | data.clear (); 95 | } 96 | 97 | template 98 | pcl::io::MedianBuffer::MedianBuffer (size_t size, 99 | size_t window_size) 100 | : Buffer (size) 101 | , window_size_ (window_size) 102 | , midpoint_ (window_size_ / 2) 103 | , data_current_idx_ (window_size_ - 1) 104 | { 105 | assert (size_ > 0); 106 | assert (window_size_ > 0 && 107 | window_size_ <= std::numeric_limits::max ()); 108 | 109 | data_.resize (window_size_); 110 | for (size_t i = 0; i < window_size_; ++i) 111 | data_[i].resize (size_, buffer_traits::invalid ()); 112 | 113 | data_argsort_indices_.resize (size_); 114 | for (size_t i = 0; i < size_; ++i) 115 | { 116 | data_argsort_indices_[i].resize (window_size_); 117 | for (size_t j = 0; j < window_size_; ++j) 118 | data_argsort_indices_[i][j] = j; 119 | } 120 | 121 | data_invalid_count_.resize (size_, window_size_); 122 | } 123 | 124 | template 125 | pcl::io::MedianBuffer::~MedianBuffer () 126 | { 127 | } 128 | 129 | template T 130 | pcl::io::MedianBuffer::operator[] (size_t idx) const 131 | { 132 | assert (idx < size_); 133 | int midpoint = (window_size_ - data_invalid_count_[idx]) / 2; 134 | return (data_[data_argsort_indices_[idx][midpoint]][idx]); 135 | } 136 | 137 | template void 138 | pcl::io::MedianBuffer::push (std::vector& data) 139 | { 140 | assert (data.size () == size_); 141 | 142 | if (++data_current_idx_ >= window_size_) 143 | data_current_idx_ = 0; 144 | 145 | // New data will replace the column with index data_current_idx_. Before 146 | // overwriting it, we go through all the new-old value pairs and update 147 | // data_argsort_indices_ to maintain sorted order. 148 | for (size_t i = 0; i < size_; ++i) 149 | { 150 | const T& new_value = data[i]; 151 | const T& old_value = data_[data_current_idx_][i]; 152 | bool new_is_nan = buffer_traits::is_invalid (new_value); 153 | bool old_is_nan = buffer_traits::is_invalid (old_value); 154 | if (compare (new_value, old_value) == 0) 155 | continue; 156 | std::vector& argsort_indices = data_argsort_indices_[i]; 157 | // Rewrite the argsort indices before or after the position where we insert 158 | // depending on the relation between the old and new values 159 | if (compare (new_value, old_value) == 1) 160 | { 161 | for (int j = 0; j < window_size_; ++j) 162 | if (argsort_indices[j] == data_current_idx_) 163 | { 164 | int k = j + 1; 165 | while (k < window_size_ && compare (new_value, data_[argsort_indices[k]][i]) == 1) 166 | { 167 | std::swap (argsort_indices[k - 1], argsort_indices[k]); 168 | ++k; 169 | } 170 | break; 171 | } 172 | } 173 | else 174 | { 175 | for (int j = window_size_ - 1; j >= 0; --j) 176 | if (argsort_indices[j] == data_current_idx_) 177 | { 178 | int k = j - 1; 179 | while (k >= 0 && compare (new_value, data_[argsort_indices[k]][i]) == -1) 180 | { 181 | std::swap (argsort_indices[k], argsort_indices[k + 1]); 182 | --k; 183 | } 184 | break; 185 | } 186 | } 187 | 188 | if (new_is_nan && !old_is_nan) 189 | ++data_invalid_count_[i]; 190 | else if (!new_is_nan && old_is_nan) 191 | --data_invalid_count_[i]; 192 | } 193 | 194 | // Finally overwrite the data 195 | data_[data_current_idx_].swap (data); 196 | data.clear (); 197 | } 198 | 199 | template int 200 | pcl::io::MedianBuffer::compare (T a, T b) 201 | { 202 | bool a_is_nan = buffer_traits::is_invalid (a); 203 | bool b_is_nan = buffer_traits::is_invalid (b); 204 | if (a_is_nan && b_is_nan) 205 | return 0; 206 | if (a_is_nan) 207 | return 1; 208 | if (b_is_nan) 209 | return -1; 210 | if (a == b) 211 | return 0; 212 | return a > b ? 1 : -1; 213 | } 214 | 215 | template 216 | pcl::io::AverageBuffer::AverageBuffer (size_t size, 217 | size_t window_size) 218 | : Buffer (size) 219 | , window_size_ (window_size) 220 | , data_current_idx_ (window_size_ - 1) 221 | { 222 | assert (size_ > 0); 223 | assert (window_size_ > 0 && 224 | window_size_ <= std::numeric_limits::max ()); 225 | 226 | data_.resize (window_size_); 227 | for (size_t i = 0; i < window_size_; ++i) 228 | data_[i].resize (size_, buffer_traits::invalid ()); 229 | 230 | data_sum_.resize (size_, 0); 231 | data_invalid_count_.resize (size_, window_size_); 232 | } 233 | 234 | template 235 | pcl::io::AverageBuffer::~AverageBuffer () 236 | { 237 | } 238 | 239 | template T 240 | pcl::io::AverageBuffer::operator[] (size_t idx) const 241 | { 242 | assert (idx < size_); 243 | if (data_invalid_count_[idx] == window_size_) 244 | return (buffer_traits::invalid ()); 245 | else 246 | return (data_sum_[idx] / (window_size_ - data_invalid_count_[idx])); 247 | } 248 | 249 | template void 250 | pcl::io::AverageBuffer::push (std::vector& data) 251 | { 252 | assert (data.size () == size_); 253 | 254 | if (++data_current_idx_ >= window_size_) 255 | data_current_idx_ = 0; 256 | 257 | // New data will replace the column with index data_current_idx_. Before 258 | // overwriting it, we go through the old values and subtract them from the 259 | // data_sum_ 260 | for (size_t i = 0; i < size_; ++i) 261 | { 262 | const float& new_value = data[i]; 263 | const float& old_value = data_[data_current_idx_][i]; 264 | bool new_is_nan = buffer_traits::is_invalid (new_value); 265 | bool old_is_nan = buffer_traits::is_invalid (old_value); 266 | 267 | if (!old_is_nan) 268 | data_sum_[i] -= old_value; 269 | if (!new_is_nan) 270 | data_sum_[i] += new_value; 271 | 272 | if (new_is_nan && !old_is_nan) 273 | ++data_invalid_count_[i]; 274 | else if (!new_is_nan && old_is_nan) 275 | --data_invalid_count_[i]; 276 | } 277 | 278 | // Finally overwrite the data 279 | data_[data_current_idx_].swap (data); 280 | data.clear (); 281 | } 282 | 283 | -------------------------------------------------------------------------------- /include/io_exception.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011 Willow Garage, Inc. 5 | * 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the copyright holder(s) nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #include 37 | 38 | #ifndef IO_EXCEPTION_H_ 39 | #define IO_EXCEPTION_H_ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | 48 | #if defined _WIN32 && defined _MSC_VER && !defined __PRETTY_FUNCTION__ 49 | #define __PRETTY_FUNCTION__ __FUNCTION__ 50 | #endif 51 | 52 | 53 | #define THROW_IO_EXCEPTION(format,...) ::pcl::io::throwIOException( __PRETTY_FUNCTION__, __FILE__, __LINE__, format , ##__VA_ARGS__ ) 54 | 55 | 56 | namespace pcl 57 | { 58 | namespace io 59 | { 60 | /** 61 | * @brief General IO exception class 62 | */ 63 | class PCL_EXPORTS IOException : public std::exception 64 | { 65 | public: 66 | IOException (const std::string& function_name, 67 | const std::string& file_name, 68 | unsigned line_number, 69 | const std::string& message); 70 | 71 | virtual ~IOException () throw (); 72 | 73 | IOException& 74 | operator= (const IOException& exception); 75 | 76 | virtual const char* 77 | what () const throw (); 78 | 79 | const std::string& 80 | getFunctionName () const; 81 | 82 | const std::string& 83 | getFileName () const; 84 | 85 | unsigned 86 | getLineNumber () const; 87 | 88 | protected: 89 | std::string function_name_; 90 | std::string file_name_; 91 | unsigned line_number_; 92 | std::string message_; 93 | std::string message_long_; 94 | }; 95 | 96 | inline void 97 | throwIOException (const char* function, const char* file, unsigned line, const char* format, ...) 98 | { 99 | static char msg[1024]; 100 | va_list args; 101 | va_start (args, format); 102 | vsnprintf (msg, 1024, format, args); 103 | throw IOException (function, file, line, msg); 104 | } 105 | } // namespace 106 | } 107 | #endif // IO_EXCEPTION_H_ 108 | -------------------------------------------------------------------------------- /include/real_sense/real_sense_device_manager.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2015-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_IO_REAL_SENSE_DEVICE_MANAGER_H 39 | #define PCL_IO_REAL_SENSE_DEVICE_MANAGER_H 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | 49 | #include 50 | #include 51 | #include 52 | 53 | namespace pcl 54 | { 55 | 56 | class RealSenseGrabber; 57 | 58 | namespace io 59 | { 60 | 61 | namespace real_sense 62 | { 63 | 64 | class RealSenseDevice; 65 | 66 | class PCL_EXPORTS RealSenseDeviceManager : boost::noncopyable 67 | { 68 | 69 | public: 70 | 71 | typedef boost::shared_ptr Ptr; 72 | 73 | static Ptr& 74 | getInstance () 75 | { 76 | static Ptr instance; 77 | if (!instance) 78 | { 79 | boost::mutex::scoped_lock lock (mutex_); 80 | if (!instance) 81 | instance.reset (new RealSenseDeviceManager); 82 | } 83 | return (instance); 84 | } 85 | 86 | inline size_t 87 | getNumDevices () 88 | { 89 | return (device_list_.size ()); 90 | } 91 | 92 | boost::shared_ptr 93 | captureDevice (); 94 | 95 | boost::shared_ptr 96 | captureDevice (size_t index); 97 | 98 | boost::shared_ptr 99 | captureDevice (const std::string& sn); 100 | 101 | ~RealSenseDeviceManager (); 102 | 103 | private: 104 | 105 | struct DeviceInfo 106 | { 107 | pxcUID iuid; 108 | pxcI32 didx; 109 | std::string serial; 110 | boost::weak_ptr device_ptr; 111 | inline bool isCaptured () { return (!device_ptr.expired ()); } 112 | }; 113 | 114 | /** If the device is already captured returns a pointer. */ 115 | boost::shared_ptr 116 | capture (DeviceInfo& device_info); 117 | 118 | RealSenseDeviceManager (); 119 | 120 | /** This function discovers devices that are capable of streaming 121 | * depth data. */ 122 | void 123 | populateDeviceList (); 124 | 125 | boost::shared_ptr session_; 126 | boost::shared_ptr capture_manager_; 127 | 128 | std::vector device_list_; 129 | 130 | static boost::mutex mutex_; 131 | 132 | }; 133 | 134 | class PCL_EXPORTS RealSenseDevice : boost::noncopyable 135 | { 136 | 137 | public: 138 | 139 | typedef boost::shared_ptr Ptr; 140 | 141 | inline const std::string& 142 | getSerialNumber () { return (device_id_); } 143 | 144 | inline PXCCapture::Device& 145 | getPXCDevice () { return (*device_); } 146 | 147 | /** Reset the state of given device by releasing and capturing again. */ 148 | static void 149 | reset (RealSenseDevice::Ptr& device) 150 | { 151 | std::string id = device->getSerialNumber (); 152 | device.reset (); 153 | device = RealSenseDeviceManager::getInstance ()->captureDevice (id); 154 | } 155 | 156 | private: 157 | 158 | friend class RealSenseDeviceManager; 159 | 160 | std::string device_id_; 161 | boost::shared_ptr capture_; 162 | boost::shared_ptr device_; 163 | 164 | RealSenseDevice (const std::string& id) : device_id_ (id) { }; 165 | 166 | }; 167 | 168 | } // namespace real_sense 169 | 170 | } // namespace io 171 | 172 | } // namespace pcl 173 | 174 | #endif /* PCL_IO_REAL_SENSE_DEVICE_MANAGER_H */ 175 | 176 | -------------------------------------------------------------------------------- /include/real_sense/time.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2014-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_IO_DEPTH_SENSE_TIME_H 39 | #define PCL_IO_DEPTH_SENSE_TIME_H 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | 46 | namespace pcl 47 | { 48 | 49 | #if PCL_VERSION < 100800 50 | 51 | /** \brief A helper class to measure frequency of a certain event. 52 | * 53 | * To use this class create an instance and call event() function every time 54 | * the event in question occurs. The estimated frequency can be retrieved 55 | * with getFrequency() function. 56 | * 57 | * \author Sergey Alexandrov 58 | * \ingroup common 59 | */ 60 | class EventFrequency 61 | { 62 | 63 | public: 64 | 65 | /** \brief Constructor. 66 | * 67 | * \param[i] window_size number of most recent events that are 68 | * considered in frequency estimation (default: 30) */ 69 | EventFrequency (size_t window_size = 30) 70 | : window_size_ (window_size) 71 | { 72 | stop_watch_.reset (); 73 | } 74 | 75 | /** \brief Notifies the class that the event occured. */ 76 | void event () 77 | { 78 | event_time_queue_.push (stop_watch_.getTimeSeconds ()); 79 | if (event_time_queue_.size () > window_size_) 80 | event_time_queue_.pop (); 81 | } 82 | 83 | /** \brief Retrieve the estimated frequency. */ 84 | double 85 | getFrequency () const 86 | { 87 | if (event_time_queue_.size () < 2) 88 | return (0.0); 89 | return ((event_time_queue_.size () - 1) / 90 | (event_time_queue_.back () - event_time_queue_.front ())); 91 | } 92 | 93 | /** \brief Reset frequency computation. */ 94 | void reset () 95 | { 96 | stop_watch_.reset (); 97 | event_time_queue_ = std::queue (); 98 | } 99 | 100 | private: 101 | 102 | pcl::StopWatch stop_watch_; 103 | std::queue event_time_queue_; 104 | const size_t window_size_; 105 | 106 | }; 107 | 108 | #endif 109 | 110 | class MyStopWatch 111 | { 112 | public: 113 | /** \brief Constructor. */ 114 | MyStopWatch () : start_time_ (boost::posix_time::microsec_clock::local_time ()) 115 | { 116 | } 117 | 118 | /** \brief Destructor. */ 119 | virtual ~MyStopWatch () {} 120 | 121 | /** \brief Retrieve the time in milliseconds spent since the last call to \a reset(). */ 122 | inline double 123 | getTime () 124 | { 125 | boost::posix_time::ptime end_time = boost::posix_time::microsec_clock::local_time (); 126 | return (static_cast (((end_time - start_time_).total_milliseconds ()))); 127 | } 128 | 129 | /** \brief Retrieve the time in seconds spent since the last call to \a reset(). */ 130 | inline double 131 | getTimeSeconds () 132 | { 133 | return (getTime () * 0.001f); 134 | } 135 | 136 | inline double 137 | getTimeMicroSeconds () 138 | { 139 | boost::posix_time::ptime end_time = boost::posix_time::microsec_clock::local_time (); 140 | return (static_cast (((end_time - start_time_).total_microseconds ()))); 141 | } 142 | 143 | /** \brief Reset the stopwatch to 0. */ 144 | inline void 145 | reset () 146 | { 147 | start_time_ = boost::posix_time::microsec_clock::local_time (); 148 | } 149 | 150 | protected: 151 | boost::posix_time::ptime start_time_; 152 | }; 153 | 154 | class MyScopeTime : public MyStopWatch 155 | { 156 | public: 157 | inline MyScopeTime (const char* title) : 158 | title_ (std::string (title)) 159 | { 160 | start_time_ = boost::posix_time::microsec_clock::local_time (); 161 | } 162 | 163 | inline MyScopeTime () : 164 | title_ (std::string ("")) 165 | { 166 | start_time_ = boost::posix_time::microsec_clock::local_time (); 167 | } 168 | 169 | inline ~MyScopeTime () 170 | { 171 | double val = this->getTimeMicroSeconds (); 172 | std::cerr << title_ << " took " << val << " microseconds.\n"; 173 | } 174 | 175 | private: 176 | std::string title_; 177 | }; 178 | 179 | } 180 | 181 | #endif /* PCL_IO_DEPTH_SENSE_TIME_H */ 182 | 183 | -------------------------------------------------------------------------------- /include/real_sense_grabber.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2015-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #ifndef PCL_IO_REAL_SENSE_GRABBER_H 39 | #define PCL_IO_REAL_SENSE_GRABBER_H 40 | 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | #include "real_sense/time.h" 49 | 50 | namespace pcl 51 | { 52 | 53 | namespace io 54 | { 55 | 56 | template class Buffer; 57 | 58 | namespace real_sense 59 | { 60 | class RealSenseDevice; 61 | } 62 | 63 | } 64 | 65 | class PCL_EXPORTS RealSenseGrabber : public Grabber 66 | { 67 | 68 | public: 69 | 70 | typedef 71 | void (sig_cb_real_sense_point_cloud) 72 | (const pcl::PointCloud::ConstPtr&); 73 | 74 | typedef 75 | void (sig_cb_real_sense_point_cloud_rgba) 76 | (const pcl::PointCloud::ConstPtr&); 77 | 78 | /** A descriptor for capturing mode. 79 | * 80 | * Consists of framerate and resolutions of depth and color streams. 81 | * Serves two purposes: to describe the desired capturing mode when 82 | * creating a grabber, and to list the available modes supported by the 83 | * grabber (see getAvailableModes()). In the first case setting some 84 | * fields to zero means "don't care", i.e. the grabber is allowed to 85 | * decide itself which concrete values to use. */ 86 | struct PCL_EXPORTS Mode 87 | { 88 | unsigned int fps; 89 | unsigned int depth_width; 90 | unsigned int depth_height; 91 | unsigned int color_width; 92 | unsigned int color_height; 93 | 94 | /** Set all fields to zero (i.e. "don't care"). */ 95 | Mode (); 96 | 97 | /** Set desired framerate, the rest is "don't care". */ 98 | Mode (unsigned int fps); 99 | 100 | /** Set desired depth resolution, the rest is "don't care". */ 101 | Mode (unsigned int depth_width, unsigned int depth_height); 102 | 103 | /** Set desired framerate and depth resolution, the rest is "don't 104 | * care". */ 105 | Mode (unsigned int fps, unsigned int depth_width, unsigned int depth_height); 106 | 107 | /** Set desired depth and color resolution, the rest is "don't 108 | * care". */ 109 | Mode (unsigned int depth_width, unsigned int depth_height, unsigned int color_width, unsigned int color_height); 110 | 111 | /** Set desired framerate, depth and color resolution. */ 112 | Mode (unsigned int fps, unsigned int depth_width, unsigned int depth_height, unsigned int color_width, unsigned int color_height); 113 | 114 | bool 115 | operator== (const pcl::RealSenseGrabber::Mode& m) const; 116 | }; 117 | 118 | enum TemporalFilteringType 119 | { 120 | RealSense_None = 0, 121 | RealSense_Median = 1, 122 | RealSense_Average = 2, 123 | }; 124 | 125 | /** Create a grabber for a RealSense device. 126 | * 127 | * The grabber "captures" the device, making it impossible for other 128 | * grabbers to interact with it. The device is "released" when the 129 | * grabber is destructed. 130 | * 131 | * This will throw pcl::io::IOException if there are no free devices 132 | * that match the supplied \a device_id. 133 | * 134 | * \param[in] device_id device identifier, which can be a serial number, 135 | * a zero-based index (with '#' prefix), or an empty string (to select 136 | * the first available device) 137 | * \param[in] mode desired framerate and stream resolution (see Mode). 138 | * If the default is supplied, then the mode closest to VGA at 30 Hz 139 | * will be chosen. 140 | * \param[in] strict if set to \c true, an exception will be thrown if 141 | * device does not support exactly the mode requsted. Otherwise the 142 | * closest available mode is selected. */ 143 | RealSenseGrabber (const std::string& device_id = "", const Mode& mode = Mode (), bool strict = false); 144 | 145 | virtual 146 | ~RealSenseGrabber () throw (); 147 | 148 | virtual void 149 | start (); 150 | 151 | virtual void 152 | stop (); 153 | 154 | virtual bool 155 | isRunning () const; 156 | 157 | virtual std::string 158 | getName () const 159 | { 160 | return (std::string ("RealSenseGrabber")); 161 | } 162 | 163 | virtual float 164 | getFramesPerSecond () const; 165 | 166 | /** Set the confidence threshold for depth data. 167 | * 168 | * Valid range is [0..15]. Discarded points will have their coordinates 169 | * set to NaNs). */ 170 | void 171 | setConfidenceThreshold (unsigned int threshold); 172 | 173 | /** Enable temporal filtering of the depth data received from the device. 174 | * 175 | * The window size parameter is not relevant for `RealSense_None` 176 | * filtering type. 177 | * 178 | * \note if the grabber is running and the new parameters are different 179 | * from the current parameters, grabber will be restarted. */ 180 | void 181 | enableTemporalFiltering (TemporalFilteringType type, size_t window_size); 182 | 183 | /** Disable temporal filtering. */ 184 | void 185 | disableTemporalFiltering (); 186 | 187 | /** Get the serial number of device captured by the grabber. */ 188 | const std::string& 189 | getDeviceSerialNumber () const; 190 | 191 | /** Get a list of capturing modes supported by the PXC device 192 | * controlled by this grabber. 193 | * 194 | * \param[in] only_depth list depth-only modes 195 | * 196 | * \note: this list exclude modes where framerates of the depth and 197 | * color streams do not match. */ 198 | std::vector 199 | getAvailableModes (bool only_depth = false) const; 200 | 201 | /** Set desired capturing mode. 202 | * 203 | * \note if the grabber is running and the new mode is different the 204 | * one requested previously, grabber will be restarted. */ 205 | void 206 | setMode (const Mode& mode, bool strict = false); 207 | 208 | /** Get currently active capturing mode. 209 | * 210 | * \note: capturing mode is selected when start() is called; output of 211 | * this function before grabber was started is undefined. */ 212 | const Mode& 213 | getMode () const 214 | { 215 | return (mode_selected_); 216 | } 217 | 218 | private: 219 | 220 | void 221 | run (); 222 | 223 | void 224 | createDepthBuffer (); 225 | 226 | void 227 | selectMode (); 228 | 229 | /** Compute a score which indicates how different is a given mode is from 230 | * the mode requested by the user. 231 | * 232 | * Importance of factors: fps > depth resolution > color resolution. The 233 | * lower the score the better. */ 234 | float 235 | computeModeScore (const Mode& mode); 236 | 237 | // Signals to indicate whether new clouds are available 238 | boost::signals2::signal* point_cloud_signal_; 239 | boost::signals2::signal* point_cloud_rgba_signal_; 240 | 241 | boost::shared_ptr device_; 242 | 243 | bool is_running_; 244 | unsigned int confidence_threshold_; 245 | 246 | TemporalFilteringType temporal_filtering_type_; 247 | size_t temporal_filtering_window_size_; 248 | 249 | /// Capture mode requested by the user at construction time 250 | Mode mode_requested_; 251 | 252 | /// Whether or not selected capture mode should strictly match what the user 253 | /// has requested 254 | bool strict_; 255 | 256 | /// Capture mode selected by grabber (among the modes supported by the 257 | /// device), computed and stored on start() 258 | Mode mode_selected_; 259 | 260 | /// Indicates whether there are subscribers for PointXYZ signal, computed 261 | /// and stored on start() 262 | bool need_xyz_; 263 | 264 | /// Indicates whether there are subscribers for PointXYZRGBA signal, 265 | /// computed and stored on start() 266 | bool need_xyzrgba_; 267 | 268 | EventFrequency frequency_; 269 | mutable boost::mutex fps_mutex_; 270 | 271 | boost::thread thread_; 272 | 273 | /// Depth buffer to perform temporal filtering of the depth images 274 | boost::shared_ptr > depth_buffer_; 275 | 276 | }; 277 | 278 | } 279 | 280 | #endif /* PCL_IO_REAL_SENSE_GRABBER_H */ 281 | 282 | -------------------------------------------------------------------------------- /src/io_exception.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2011 Willow Garage, Inc. 5 | * 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of Willow Garage, Inc. nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | */ 36 | #include "pcl/io/io_exception.h" 37 | #include 38 | 39 | pcl::io::IOException::IOException (const std::string& function_name, const std::string& file_name, unsigned line_number, const std::string& message) 40 | : function_name_ (function_name) 41 | , file_name_ (file_name) 42 | , line_number_ (line_number) 43 | , message_ (message) 44 | { 45 | std::stringstream sstream; 46 | sstream << function_name_ << " @ " << file_name_ << " @ " << line_number_ << " : " << message_; 47 | message_long_ = sstream.str (); 48 | } 49 | 50 | pcl::io::IOException::~IOException () throw () 51 | { 52 | } 53 | 54 | pcl::io::IOException& 55 | pcl::io::IOException::operator = (const IOException& exception) 56 | { 57 | message_ = exception.message_; 58 | return (*this); 59 | } 60 | 61 | const char* 62 | pcl::io::IOException::what () const throw () 63 | { 64 | return (message_long_.c_str ()); 65 | } 66 | 67 | const std::string& 68 | pcl::io::IOException::getFunctionName () const 69 | { 70 | return (function_name_); 71 | } 72 | 73 | const std::string& 74 | pcl::io::IOException::getFileName () const 75 | { 76 | return (file_name_); 77 | } 78 | 79 | unsigned 80 | pcl::io::IOException::getLineNumber () const 81 | { 82 | return (line_number_); 83 | } 84 | -------------------------------------------------------------------------------- /src/real_sense/real_sense_device_manager.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2015-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #include "io_exception.h" 39 | 40 | #include "real_sense/real_sense_device_manager.h" 41 | 42 | using namespace pcl::io; 43 | 44 | boost::mutex pcl::io::real_sense::RealSenseDeviceManager::mutex_; 45 | 46 | /** Helper function to release a RealSense resource. 47 | Useful as a deleter for a shared pointer holding RealSense resource. */ 48 | template void 49 | releasePXCResource (T* resource) 50 | { 51 | if (resource) 52 | { 53 | resource->Release (); 54 | resource = 0; 55 | } 56 | } 57 | 58 | template boost::shared_ptr 59 | makePXCSharedPtr (T* resource) 60 | { 61 | return boost::shared_ptr (resource, releasePXCResource); 62 | } 63 | 64 | boost::shared_ptr 65 | createPXCSession () 66 | { 67 | PXCSession* s = PXCSession::CreateInstance (); 68 | if (!s) 69 | THROW_IO_EXCEPTION ("failed to create RealSense session"); 70 | return makePXCSharedPtr (s); 71 | } 72 | 73 | boost::shared_ptr 74 | createPXCCaptureManager (PXCSession& session) 75 | { 76 | PXCCaptureManager* cm = session.CreateCaptureManager (); 77 | if (!cm) 78 | THROW_IO_EXCEPTION ("failed to create RealSense capture manager"); 79 | return makePXCSharedPtr (cm); 80 | } 81 | 82 | boost::shared_ptr 83 | createPXCCapture (PXCSession& session, pxcUID iuid) 84 | { 85 | PXCCapture* c; 86 | if (session.CreateImpl (iuid, &c) < PXC_STATUS_NO_ERROR) 87 | THROW_IO_EXCEPTION ("unable to create RealSense capture"); 88 | return makePXCSharedPtr (c); 89 | } 90 | 91 | boost::shared_ptr 92 | createPXCCaptureDevice (PXCCapture& capture, pxcI32 didx) 93 | { 94 | PXCCapture::Device* d; 95 | d = capture.CreateDevice (didx); 96 | if (!d) 97 | THROW_IO_EXCEPTION ("unable to create RealSense capture device"); 98 | return makePXCSharedPtr (d); 99 | } 100 | 101 | /** Utility function to convert RealSense-style strings (which happen to 102 | * consist of 2-byte chars) into standard library strings. */ 103 | std::string 104 | toString (const pxcCHAR* pxc_string, size_t max_length) 105 | { 106 | size_t i = 0; 107 | while (i + 1 < max_length && pxc_string[i]) 108 | ++i; 109 | std::string out (i + 1, '\0'); 110 | size_t j = 0; 111 | while (j < i) 112 | out[j] = pxc_string[j++]; 113 | return out; 114 | } 115 | 116 | pcl::io::real_sense::RealSenseDeviceManager::RealSenseDeviceManager () 117 | { 118 | session_ = createPXCSession (); 119 | populateDeviceList (); 120 | } 121 | 122 | pcl::io::real_sense::RealSenseDeviceManager::~RealSenseDeviceManager () 123 | { 124 | } 125 | 126 | pcl::io::real_sense::RealSenseDevice::Ptr 127 | pcl::io::real_sense::RealSenseDeviceManager::captureDevice () 128 | { 129 | boost::mutex::scoped_lock lock (mutex_); 130 | if (device_list_.size () == 0) 131 | THROW_IO_EXCEPTION ("no connected devices"); 132 | for (size_t i = 0; i < device_list_.size (); ++i) 133 | if (!device_list_[i].isCaptured ()) 134 | return (capture (device_list_[i])); 135 | THROW_IO_EXCEPTION ("all connected devices are captured by other grabbers"); 136 | return (RealSenseDevice::Ptr ()); // never reached, needed just to silence -Wreturn-type warning 137 | } 138 | 139 | pcl::io::real_sense::RealSenseDevice::Ptr 140 | pcl::io::real_sense::RealSenseDeviceManager::captureDevice (size_t index) 141 | { 142 | boost::mutex::scoped_lock lock (mutex_); 143 | if (index >= device_list_.size ()) 144 | THROW_IO_EXCEPTION ("device with index %i is not connected", index + 1); 145 | if (device_list_[index].isCaptured ()) 146 | THROW_IO_EXCEPTION ("device with index %i is captured by another grabber", index + 1); 147 | return (capture (device_list_[index])); 148 | } 149 | 150 | pcl::io::real_sense::RealSenseDevice::Ptr 151 | pcl::io::real_sense::RealSenseDeviceManager::captureDevice (const std::string& sn) 152 | { 153 | boost::mutex::scoped_lock lock (mutex_); 154 | for (size_t i = 0; i < device_list_.size (); ++i) 155 | { 156 | if (device_list_[i].serial == sn) 157 | { 158 | if (device_list_[i].isCaptured ()) 159 | THROW_IO_EXCEPTION ("device with serial number %s is captured by another grabber", sn.c_str ()); 160 | return (capture (device_list_[i])); 161 | } 162 | } 163 | THROW_IO_EXCEPTION ("device with serial number %s is not connected", sn.c_str ()); 164 | return (RealSenseDevice::Ptr ()); // never reached, needed just to silence -Wreturn-type warning 165 | } 166 | 167 | void 168 | pcl::io::real_sense::RealSenseDeviceManager::populateDeviceList () 169 | { 170 | device_list_.clear (); 171 | 172 | // Module description to match 173 | PXCSession::ImplDesc module_desc = {}; 174 | module_desc.group = PXCSession::IMPL_GROUP_SENSOR; 175 | module_desc.subgroup = PXCSession::IMPL_SUBGROUP_VIDEO_CAPTURE; 176 | 177 | for (int m = 0;; m++) 178 | { 179 | PXCSession::ImplDesc desc; 180 | if (session_->QueryImpl (&module_desc, m, &desc) < PXC_STATUS_NO_ERROR) 181 | break; 182 | PXCCapture* capture; 183 | if (session_->CreateImpl (&desc, &capture) < PXC_STATUS_NO_ERROR) 184 | continue; 185 | for (int j = 0;; j++) 186 | { 187 | PXCCapture::DeviceInfo device_info; 188 | if (capture->QueryDeviceInfo (j, &device_info) < PXC_STATUS_NO_ERROR) 189 | break; 190 | if (device_info.streams & PXCCapture::STREAM_TYPE_DEPTH) 191 | { 192 | const size_t MAX_SERIAL_LENGTH = sizeof (device_info.serial) / sizeof (device_info.serial[0]); 193 | std::string serial = toString (device_info.serial, MAX_SERIAL_LENGTH); 194 | device_list_.push_back (DeviceInfo ()); 195 | device_list_.back ().serial = serial; 196 | device_list_.back ().iuid = desc.iuid; 197 | device_list_.back ().didx = j; 198 | } 199 | } 200 | capture->Release (); 201 | } 202 | } 203 | 204 | pcl::io::real_sense::RealSenseDevice::Ptr 205 | pcl::io::real_sense::RealSenseDeviceManager::capture (DeviceInfo& device_info) 206 | { 207 | // This is called from public captureDevice() functions and should already be 208 | // under scoped lock 209 | if (!device_info.device_ptr.expired ()) 210 | { 211 | return device_info.device_ptr.lock (); 212 | } 213 | else 214 | { 215 | RealSenseDevice::Ptr device (new RealSenseDevice (device_info.serial)); 216 | device->capture_ = createPXCCapture (*session_, device_info.iuid); 217 | device->device_ = createPXCCaptureDevice (*device->capture_, device_info.didx); 218 | device_info.device_ptr = device; 219 | return device; 220 | } 221 | } 222 | -------------------------------------------------------------------------------- /src/real_sense_grabber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2015-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | 48 | #include "real_sense_grabber.h" 49 | #include "real_sense/real_sense_device_manager.h" 50 | #include "buffers.h" 51 | #include "io_exception.h" 52 | 53 | using namespace pcl::io; 54 | using namespace pcl::io::real_sense; 55 | 56 | /* Helper function to convert a PXCPoint3DF32 point into a PCL point. 57 | * Takes care of unit conversion (PXC point coordinates are in millimeters) 58 | * and invalid points. */ 59 | template inline void 60 | convertPoint (const PXCPoint3DF32& src, T& tgt) 61 | { 62 | static const float nan = std::numeric_limits::quiet_NaN (); 63 | if (src.z == 0) 64 | { 65 | tgt.x = tgt.y = tgt.z = nan; 66 | } 67 | else 68 | { 69 | tgt.x = -src.x / 1000.0; 70 | tgt.y = src.y / 1000.0; 71 | tgt.z = src.z / 1000.0; 72 | } 73 | } 74 | 75 | pcl::RealSenseGrabber::Mode::Mode () 76 | : fps (0), depth_width (0), depth_height (0), color_width (0), color_height (0) 77 | { 78 | } 79 | 80 | pcl::RealSenseGrabber::Mode::Mode (unsigned int f) 81 | : fps (f), depth_width (0), depth_height (0), color_width (0), color_height (0) 82 | { 83 | } 84 | 85 | pcl::RealSenseGrabber::Mode::Mode (unsigned int dw, unsigned int dh) 86 | : fps (0), depth_width (dw), depth_height (dh), color_width (0), color_height (0) 87 | { 88 | } 89 | 90 | pcl::RealSenseGrabber::Mode::Mode (unsigned int f, unsigned int dw, unsigned int dh) 91 | : fps (f), depth_width (dw), depth_height (dh), color_width (0), color_height (0) 92 | { 93 | } 94 | 95 | pcl::RealSenseGrabber::Mode::Mode (unsigned int dw, unsigned int dh, unsigned int cw, unsigned int ch) 96 | : fps (0), depth_width (dw), depth_height (dh), color_width (cw), color_height (ch) 97 | { 98 | } 99 | 100 | pcl::RealSenseGrabber::Mode::Mode (unsigned int f, unsigned int dw, unsigned int dh, unsigned int cw, unsigned int ch) 101 | : fps (f), depth_width (dw), depth_height (dh), color_width (cw), color_height (ch) 102 | { 103 | } 104 | 105 | bool 106 | pcl::RealSenseGrabber::Mode::operator== (const pcl::RealSenseGrabber::Mode& m) const 107 | { 108 | return (this->fps == m.fps && 109 | this->depth_width == m.depth_width && 110 | this->depth_height == m.depth_height && 111 | this->color_width == m.color_width && 112 | this->color_height == m.color_height); 113 | } 114 | 115 | pcl::RealSenseGrabber::RealSenseGrabber (const std::string& device_id, const Mode& mode, bool strict) 116 | : Grabber () 117 | , is_running_ (false) 118 | , confidence_threshold_ (6) 119 | , temporal_filtering_type_ (RealSense_None) 120 | , temporal_filtering_window_size_ (1) 121 | , mode_requested_ (mode) 122 | , strict_ (strict) 123 | { 124 | if (device_id == "") 125 | device_ = RealSenseDeviceManager::getInstance ()->captureDevice (); 126 | else if (device_id[0] == '#') 127 | device_ = RealSenseDeviceManager::getInstance ()->captureDevice (boost::lexical_cast (device_id.substr (1)) - 1); 128 | else 129 | device_ = RealSenseDeviceManager::getInstance ()->captureDevice (device_id); 130 | 131 | point_cloud_signal_ = createSignal (); 132 | point_cloud_rgba_signal_ = createSignal (); 133 | } 134 | 135 | pcl::RealSenseGrabber::~RealSenseGrabber () throw () 136 | { 137 | stop (); 138 | 139 | disconnect_all_slots (); 140 | disconnect_all_slots (); 141 | } 142 | 143 | void 144 | pcl::RealSenseGrabber::start () 145 | { 146 | if (!is_running_) 147 | { 148 | need_xyz_ = num_slots () > 0; 149 | need_xyzrgba_ = num_slots () > 0; 150 | if (need_xyz_ || need_xyzrgba_) 151 | { 152 | selectMode (); 153 | PXCCapture::Device::StreamProfileSet profile; 154 | memset (&profile, 0, sizeof (profile)); 155 | profile.depth.frameRate.max = mode_selected_.fps; 156 | profile.depth.frameRate.min = mode_selected_.fps; 157 | profile.depth.imageInfo.width = mode_selected_.depth_width; 158 | profile.depth.imageInfo.height = mode_selected_.depth_height; 159 | profile.depth.imageInfo.format = PXCImage::PIXEL_FORMAT_DEPTH; 160 | profile.depth.options = PXCCapture::Device::STREAM_OPTION_ANY; 161 | if (need_xyzrgba_) 162 | { 163 | profile.color.frameRate.max = mode_selected_.fps; 164 | profile.color.frameRate.min = mode_selected_.fps; 165 | profile.color.imageInfo.width = mode_selected_.color_width; 166 | profile.color.imageInfo.height = mode_selected_.color_height; 167 | profile.color.imageInfo.format = PXCImage::PIXEL_FORMAT_RGB32; 168 | profile.color.options = PXCCapture::Device::STREAM_OPTION_ANY; 169 | } 170 | device_->getPXCDevice ().SetStreamProfileSet (&profile); 171 | if (!device_->getPXCDevice ().IsStreamProfileSetValid (&profile)) 172 | THROW_IO_EXCEPTION ("Invalid stream profile for PXC device"); 173 | frequency_.reset (); 174 | is_running_ = true; 175 | thread_ = boost::thread (&RealSenseGrabber::run, this); 176 | } 177 | } 178 | } 179 | 180 | void 181 | pcl::RealSenseGrabber::stop () 182 | { 183 | if (is_running_) 184 | { 185 | is_running_ = false; 186 | thread_.join (); 187 | } 188 | } 189 | 190 | bool 191 | pcl::RealSenseGrabber::isRunning () const 192 | { 193 | return (is_running_); 194 | } 195 | 196 | float 197 | pcl::RealSenseGrabber::getFramesPerSecond () const 198 | { 199 | boost::mutex::scoped_lock lock (fps_mutex_); 200 | return (frequency_.getFrequency ()); 201 | } 202 | 203 | void 204 | pcl::RealSenseGrabber::setConfidenceThreshold (unsigned int threshold) 205 | { 206 | if (threshold > 15) 207 | { 208 | PCL_WARN ("[pcl::RealSenseGrabber::setConfidenceThreshold] Attempted to set threshold outside valid range (0-15)"); 209 | } 210 | else 211 | { 212 | confidence_threshold_ = threshold; 213 | device_->getPXCDevice ().SetDepthConfidenceThreshold (confidence_threshold_); 214 | } 215 | } 216 | 217 | void 218 | pcl::RealSenseGrabber::enableTemporalFiltering (TemporalFilteringType type, size_t window_size) 219 | { 220 | if (temporal_filtering_type_ != type || 221 | (type != RealSense_None && temporal_filtering_window_size_ != window_size)) 222 | { 223 | temporal_filtering_type_ = type; 224 | temporal_filtering_window_size_ = window_size; 225 | if (is_running_) 226 | { 227 | stop (); 228 | start (); 229 | } 230 | } 231 | } 232 | 233 | void 234 | pcl::RealSenseGrabber::disableTemporalFiltering () 235 | { 236 | enableTemporalFiltering (RealSense_None, 1); 237 | } 238 | 239 | const std::string& 240 | pcl::RealSenseGrabber::getDeviceSerialNumber () const 241 | { 242 | return (device_->getSerialNumber ()); 243 | } 244 | 245 | std::vector 246 | pcl::RealSenseGrabber::getAvailableModes (bool only_depth) const 247 | { 248 | std::vector modes; 249 | PXCCapture::StreamType streams = only_depth 250 | ? PXCCapture::STREAM_TYPE_DEPTH 251 | : PXCCapture::STREAM_TYPE_DEPTH | PXCCapture::STREAM_TYPE_COLOR; 252 | for (int p = 0;; p++) 253 | { 254 | PXCCapture::Device::StreamProfileSet profiles = {}; 255 | if (device_->getPXCDevice ().QueryStreamProfileSet (streams, p, &profiles) == PXC_STATUS_NO_ERROR) 256 | { 257 | if (!only_depth && profiles.depth.frameRate.max != profiles.color.frameRate.max) 258 | continue; // we need both streams to have the same framerate 259 | Mode mode; 260 | mode.fps = profiles.depth.frameRate.max; 261 | mode.depth_width = profiles.depth.imageInfo.width; 262 | mode.depth_height = profiles.depth.imageInfo.height; 263 | mode.color_width = profiles.color.imageInfo.width; 264 | mode.color_height = profiles.color.imageInfo.height; 265 | bool duplicate = false; 266 | for (size_t i = 0; i < modes.size (); ++i) 267 | duplicate |= modes[i] == mode; 268 | if (!duplicate) 269 | modes.push_back (mode); 270 | } 271 | else 272 | { 273 | break; 274 | } 275 | } 276 | return modes; 277 | } 278 | 279 | void 280 | pcl::RealSenseGrabber::setMode (const Mode& mode, bool strict) 281 | { 282 | if (mode == mode_requested_ && strict == strict_) 283 | return; 284 | mode_requested_ = mode; 285 | strict_ = strict; 286 | if (is_running_) 287 | { 288 | stop (); 289 | start (); 290 | } 291 | } 292 | 293 | void 294 | pcl::RealSenseGrabber::run () 295 | { 296 | const int WIDTH = mode_selected_.depth_width; 297 | const int HEIGHT = mode_selected_.depth_height; 298 | const int SIZE = WIDTH * HEIGHT; 299 | 300 | PXCProjection* projection = device_->getPXCDevice ().CreateProjection (); 301 | PXCCapture::Sample sample; 302 | std::vector vertices (SIZE); 303 | createDepthBuffer (); 304 | 305 | while (is_running_) 306 | { 307 | pcl::PointCloud::Ptr xyz_cloud; 308 | pcl::PointCloud::Ptr xyzrgba_cloud; 309 | 310 | pxcStatus status; 311 | if (need_xyzrgba_) 312 | status = device_->getPXCDevice ().ReadStreams (PXCCapture::STREAM_TYPE_DEPTH | PXCCapture::STREAM_TYPE_COLOR, &sample); 313 | else 314 | status = device_->getPXCDevice ().ReadStreams (PXCCapture::STREAM_TYPE_DEPTH, &sample); 315 | 316 | uint64_t timestamp = pcl::getTime () * 1.0e+6; 317 | 318 | switch (status) 319 | { 320 | case PXC_STATUS_NO_ERROR: 321 | { 322 | fps_mutex_.lock (); 323 | frequency_.event (); 324 | fps_mutex_.unlock (); 325 | 326 | /* We preform the following steps to convert received data into point clouds: 327 | * 328 | * 1. Push depth image to the depth buffer 329 | * 2. Pull filtered depth image from the depth buffer 330 | * 3. Project (filtered) depth image into 3D 331 | * 4. Fill XYZ point cloud with computed points 332 | * 5. Fill XYZRGBA point cloud with computed points 333 | * 7. Project color image into 3D 334 | * 6. Assign colors to points in XYZRGBA point cloud 335 | * 336 | * Steps 1-2 are skipped if temporal filtering is disabled. 337 | * Step 4 is skipped if there are no subscribers for XYZ clouds. 338 | * Steps 5-7 are skipped if there are no subscribers for XYZRGBA clouds. */ 339 | 340 | if (temporal_filtering_type_ != RealSense_None) 341 | { 342 | PXCImage::ImageData data; 343 | sample.depth->AcquireAccess (PXCImage::ACCESS_READ, &data); 344 | std::vector data_copy (SIZE); 345 | memcpy (data_copy.data (), data.planes[0], SIZE * sizeof (unsigned short)); 346 | sample.depth->ReleaseAccess (&data); 347 | 348 | depth_buffer_->push (data_copy); 349 | 350 | sample.depth->AcquireAccess (PXCImage::ACCESS_WRITE, &data); 351 | unsigned short* d = reinterpret_cast (data.planes[0]); 352 | for (size_t i = 0; i < SIZE; i++) 353 | d[i] = (*depth_buffer_)[i]; 354 | sample.depth->ReleaseAccess (&data); 355 | } 356 | 357 | projection->QueryVertices (sample.depth, vertices.data ()); 358 | 359 | if (need_xyz_) 360 | { 361 | xyz_cloud.reset (new pcl::PointCloud (WIDTH, HEIGHT)); 362 | xyz_cloud->header.stamp = timestamp; 363 | xyz_cloud->is_dense = false; 364 | for (int i = 0; i < SIZE; i++) 365 | convertPoint (vertices[i], xyz_cloud->points[i]); 366 | } 367 | 368 | if (need_xyzrgba_) 369 | { 370 | PXCImage::ImageData data; 371 | PXCImage* mapped = projection->CreateColorImageMappedToDepth (sample.depth, sample.color); 372 | mapped->AcquireAccess (PXCImage::ACCESS_READ, &data); 373 | uint32_t* d = reinterpret_cast (data.planes[0]); 374 | if (need_xyz_) 375 | { 376 | // We can fill XYZ coordinates more efficiently using pcl::copyPointCloud, 377 | // given that they were already computed for XYZ point cloud. 378 | xyzrgba_cloud.reset (new pcl::PointCloud); 379 | pcl::copyPointCloud (*xyz_cloud, *xyzrgba_cloud); 380 | for (int i = 0; i < HEIGHT; i++) 381 | { 382 | pcl::PointXYZRGBA* cloud_row = &xyzrgba_cloud->points[i * WIDTH]; 383 | uint32_t* color_row = &d[i * data.pitches[0] / sizeof (uint32_t)]; 384 | for (int j = 0; j < WIDTH; j++) 385 | memcpy (&cloud_row[j].rgba, &color_row[j], sizeof (uint32_t)); 386 | } 387 | } 388 | else 389 | { 390 | xyzrgba_cloud.reset (new pcl::PointCloud (WIDTH, HEIGHT)); 391 | xyzrgba_cloud->header.stamp = timestamp; 392 | xyzrgba_cloud->is_dense = false; 393 | for (int i = 0; i < HEIGHT; i++) 394 | { 395 | PXCPoint3DF32* vertices_row = &vertices[i * WIDTH]; 396 | pcl::PointXYZRGBA* cloud_row = &xyzrgba_cloud->points[i * WIDTH]; 397 | uint32_t* color_row = &d[i * data.pitches[0] / sizeof (uint32_t)]; 398 | for (int j = 0; j < WIDTH; j++) 399 | { 400 | convertPoint (vertices_row[j], cloud_row[j]); 401 | memcpy (&cloud_row[j].rgba, &color_row[j], sizeof (uint32_t)); 402 | } 403 | } 404 | } 405 | mapped->ReleaseAccess (&data); 406 | mapped->Release (); 407 | } 408 | 409 | if (need_xyzrgba_) 410 | point_cloud_rgba_signal_->operator () (xyzrgba_cloud); 411 | if (need_xyz_) 412 | point_cloud_signal_->operator () (xyz_cloud); 413 | break; 414 | } 415 | case PXC_STATUS_DEVICE_LOST: 416 | THROW_IO_EXCEPTION ("failed to read data stream from PXC device: device lost"); 417 | case PXC_STATUS_ALLOC_FAILED: 418 | THROW_IO_EXCEPTION ("failed to read data stream from PXC device: alloc failed"); 419 | } 420 | sample.ReleaseImages (); 421 | } 422 | projection->Release (); 423 | RealSenseDevice::reset (device_); 424 | } 425 | 426 | float 427 | pcl::RealSenseGrabber::computeModeScore (const Mode& mode) 428 | { 429 | const float FPS_WEIGHT = 100000; 430 | const float DEPTH_WEIGHT = 1000; 431 | const float COLOR_WEIGHT = 1; 432 | int f = mode.fps - mode_requested_.fps; 433 | int dw = mode.depth_width - mode_requested_.depth_width; 434 | int dh = mode.depth_height - mode_requested_.depth_height; 435 | int cw = mode.color_width - mode_requested_.color_width; 436 | int ch = mode.color_height - mode_requested_.color_height; 437 | float penalty; 438 | penalty = std::abs (FPS_WEIGHT * f * (mode_requested_.fps != 0)); 439 | penalty += std::abs (DEPTH_WEIGHT * dw * (mode_requested_.depth_width != 0)); 440 | penalty += std::abs (DEPTH_WEIGHT * dh * (mode_requested_.depth_height != 0)); 441 | penalty += std::abs (COLOR_WEIGHT * cw * (mode_requested_.color_width != 0 && need_xyzrgba_)); 442 | penalty += std::abs (COLOR_WEIGHT * ch * (mode_requested_.color_height != 0 && need_xyzrgba_)); 443 | return penalty; 444 | } 445 | 446 | void 447 | pcl::RealSenseGrabber::selectMode () 448 | { 449 | if (mode_requested_ == Mode ()) 450 | mode_requested_ = Mode (30, 640, 480, 640, 480); 451 | float best_score = std::numeric_limits::max (); 452 | std::vector modes = getAvailableModes (!need_xyzrgba_); 453 | for (size_t i = 0; i < modes.size (); ++i) 454 | { 455 | Mode mode = modes[i]; 456 | float score = computeModeScore (mode); 457 | if (score < best_score) 458 | { 459 | best_score = score; 460 | mode_selected_ = mode; 461 | } 462 | } 463 | if (strict_ && best_score > 0) 464 | THROW_IO_EXCEPTION ("PXC device does not support requested mode"); 465 | } 466 | 467 | void 468 | pcl::RealSenseGrabber::createDepthBuffer () 469 | { 470 | size_t size = mode_selected_.depth_width * mode_selected_.depth_height; 471 | switch (temporal_filtering_type_) 472 | { 473 | case RealSense_None: 474 | { 475 | depth_buffer_.reset (new pcl::io::SingleBuffer (size)); 476 | break; 477 | } 478 | case RealSense_Median: 479 | { 480 | depth_buffer_.reset (new pcl::io::MedianBuffer (size, temporal_filtering_window_size_)); 481 | break; 482 | } 483 | case RealSense_Average: 484 | { 485 | depth_buffer_.reset (new pcl::io::AverageBuffer (size, temporal_filtering_window_size_)); 486 | break; 487 | } 488 | } 489 | } 490 | -------------------------------------------------------------------------------- /src/real_sense_viewer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Point Cloud Library (PCL) - www.pointclouds.org 5 | * Copyright (c) 2015-, Open Perception, Inc. 6 | * 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the copyright holder(s) nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | */ 37 | 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | #include "real_sense_grabber.h" 53 | 54 | using namespace pcl::console; 55 | 56 | void 57 | printHelp (int, char **argv) 58 | { 59 | std::cout << std::endl; 60 | std::cout << "****************************************************************************" << std::endl; 61 | std::cout << "* *" << std::endl; 62 | std::cout << "* REAL SENSE VIEWER - Usage Guide *" << std::endl; 63 | std::cout << "* *" << std::endl; 64 | std::cout << "****************************************************************************" << std::endl; 65 | std::cout << std::endl; 66 | std::cout << "Usage: " << argv[0] << " [Options] device_id" << std::endl; 67 | std::cout << std::endl; 68 | std::cout << "Options:" << std::endl; 69 | std::cout << std::endl; 70 | std::cout << " --help, -h : Show this help" << std::endl; 71 | std::cout << " --list, -l : List connected RealSense devices and supported modes" << std::endl; 72 | std::cout << " --mode : Use capture mode from the list of supported modes" << std::endl; 73 | std::cout << std::endl; 74 | std::cout << "Keyboard commands:" << std::endl; 75 | std::cout << std::endl; 76 | std::cout << " When the focus is on the viewer window, the following keyboard commands" << std::endl; 77 | std::cout << " are available:" << std::endl; 78 | std::cout << " * t/T : increase or decrease depth data confidence threshold" << std::endl; 79 | std::cout << " * k : enable next temporal filtering method" << std::endl; 80 | std::cout << " * b : toggle bilateral filtering" << std::endl; 81 | std::cout << " * a/A : increase or decrease bilateral filter spatial sigma" << std::endl; 82 | std::cout << " * z/Z : increase or decrease bilateral filter range sigma" << std::endl; 83 | std::cout << " * s : save the last grabbed cloud to disk" << std::endl; 84 | std::cout << " * h : print the list of standard PCL viewer commands" << std::endl; 85 | std::cout << std::endl; 86 | std::cout << "Notes:" << std::endl; 87 | std::cout << std::endl; 88 | std::cout << " The device to grab data from is selected using device_id argument. It" << std::endl; 89 | std::cout << " could be either:" << std::endl; 90 | std::cout << " * serial number (e.g. 231400041-03)" << std::endl; 91 | std::cout << " * device index (e.g. #2 for the second connected device)" << std::endl; 92 | std::cout << std::endl; 93 | std::cout << " If device_id is not given, then the first available device will be used." << std::endl; 94 | std::cout << std::endl; 95 | std::cout << " If capture mode is not given, then the grabber will try to enable both" << std::endl; 96 | std::cout << " depth and color streams at VGA resolution and 30 Hz framerate. If this" << std::endl; 97 | std::cout << " particular mode is not available, the one that most closely matches this" << std::endl; 98 | std::cout << " specification will be chosen." << std::endl; 99 | std::cout << std::endl; 100 | } 101 | 102 | void 103 | printDeviceList () 104 | { 105 | typedef boost::shared_ptr RealSenseGrabberPtr; 106 | std::vector grabbers; 107 | std::cout << "Connected devices: "; 108 | boost::format fmt ("\n #%i %s"); 109 | boost::format fmt_dm ("\n %2i) %d Hz %dx%d Depth"); 110 | boost::format fmt_dcm ("\n %2i) %d Hz %dx%d Depth %dx%d Color"); 111 | while (true) 112 | { 113 | try 114 | { 115 | grabbers.push_back (RealSenseGrabberPtr (new pcl::RealSenseGrabber)); 116 | std::cout << boost::str (fmt % grabbers.size () % grabbers.back ()->getDeviceSerialNumber ()); 117 | std::vector xyz_modes = grabbers.back ()->getAvailableModes (true); 118 | std::cout << "\n Depth modes:"; 119 | if (xyz_modes.size ()) 120 | for (size_t i = 0; i < xyz_modes.size (); ++i) 121 | std::cout << boost::str (fmt_dm % (i + 1) % xyz_modes[i].fps % xyz_modes[i].depth_width % xyz_modes[i].depth_height); 122 | else 123 | { 124 | std::cout << " none"; 125 | } 126 | std::vector xyzrgba_modes = grabbers.back ()->getAvailableModes (false); 127 | std::cout << "\n Depth + color modes:"; 128 | if (xyz_modes.size ()) 129 | for (size_t i = 0; i < xyzrgba_modes.size (); ++i) 130 | { 131 | const pcl::RealSenseGrabber::Mode& m = xyzrgba_modes[i]; 132 | std::cout << boost::str (fmt_dcm % (i + xyz_modes.size () + 1) % m.fps % m.depth_width % m.depth_height % m.color_width % m.color_height); 133 | } 134 | else 135 | std::cout << " none"; 136 | } 137 | catch (pcl::io::IOException& e) 138 | { 139 | break; 140 | } 141 | } 142 | if (grabbers.size ()) 143 | std::cout << std::endl; 144 | else 145 | std::cout << "none" << std::endl; 146 | } 147 | 148 | template 149 | class RealSenseViewer 150 | { 151 | 152 | public: 153 | 154 | typedef pcl::PointCloud PointCloudT; 155 | 156 | RealSenseViewer (pcl::RealSenseGrabber& grabber) 157 | : grabber_ (grabber) 158 | , viewer_ ("RealSense Viewer") 159 | , window_ (3) 160 | , threshold_ (6) 161 | , temporal_filtering_ (pcl::RealSenseGrabber::RealSense_None) 162 | , with_bilateral_ (false) 163 | { 164 | viewer_.setCameraFieldOfView (0.785398); // approximately 45 degrees 165 | viewer_.setCameraPosition (0, 0, 0, 0, 0, 1, 0, 1, 0); 166 | viewer_.registerKeyboardCallback (&RealSenseViewer::keyboardCallback, *this); 167 | viewer_.registerPointPickingCallback (&RealSenseViewer::pointPickingCallback, *this); 168 | bilateral_.setSigmaS (5); 169 | } 170 | 171 | ~RealSenseViewer () 172 | { 173 | connection_.disconnect (); 174 | } 175 | 176 | void 177 | run () 178 | { 179 | boost::function f = boost::bind (&RealSenseViewer::cloudCallback, this, _1); 180 | connection_ = grabber_.registerCallback (f); 181 | grabber_.start (); 182 | printMode (grabber_.getMode ()); 183 | // TODO: the function below does not function in PCL 1.7.2, but is fixed in master; uncomment when merging. 184 | // viewer_.setSize (grabber_.getMode ().depth_width, grabber_.getMode ().depth_height); 185 | while (!viewer_.wasStopped ()) 186 | { 187 | if (new_cloud_) 188 | { 189 | boost::mutex::scoped_lock lock (new_cloud_mutex_); 190 | if (!viewer_.updatePointCloud (new_cloud_, "cloud")) 191 | { 192 | viewer_.addPointCloud (new_cloud_, "cloud"); 193 | viewer_.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud"); 194 | } 195 | displaySettings (); 196 | last_cloud_ = new_cloud_; 197 | new_cloud_.reset (); 198 | } 199 | viewer_.spinOnce (1, true); 200 | } 201 | grabber_.stop (); 202 | } 203 | 204 | private: 205 | 206 | void 207 | cloudCallback (typename PointCloudT::ConstPtr cloud) 208 | { 209 | if (!viewer_.wasStopped ()) 210 | { 211 | boost::mutex::scoped_lock lock (new_cloud_mutex_); 212 | if (with_bilateral_) 213 | { 214 | bilateral_.setInputCloud (cloud); 215 | typename PointCloudT::Ptr filtered (new PointCloudT); 216 | bilateral_.filter (*filtered); 217 | new_cloud_ = filtered; 218 | } 219 | else 220 | { 221 | new_cloud_ = cloud; 222 | } 223 | } 224 | } 225 | 226 | void 227 | keyboardCallback (const pcl::visualization::KeyboardEvent& event, void*) 228 | { 229 | if (event.keyDown ()) 230 | { 231 | if (event.getKeyCode () == 'w' || event.getKeyCode () == 'W') 232 | { 233 | window_ += event.getKeyCode () == 'w' ? 1 : -1; 234 | if (window_ < 1) 235 | window_ = 1; 236 | pcl::console::print_info ("Temporal filtering window size: "); 237 | pcl::console::print_value ("%i\n", window_); 238 | grabber_.enableTemporalFiltering (temporal_filtering_, window_); 239 | } 240 | if (event.getKeyCode () == 't' || event.getKeyCode () == 'T') 241 | { 242 | threshold_ += event.getKeyCode () == 't' ? 1 : -1; 243 | if (threshold_ < 0) 244 | threshold_ = 0; 245 | if (threshold_ > 15) 246 | threshold_ = 15; 247 | pcl::console::print_info ("Confidence threshold: "); 248 | pcl::console::print_value ("%i\n", threshold_); 249 | grabber_.setConfidenceThreshold (threshold_); 250 | } 251 | if (event.getKeyCode () == 'k') 252 | { 253 | pcl::console::print_info ("Temporal filtering: "); 254 | switch (temporal_filtering_) 255 | { 256 | case pcl::RealSenseGrabber::RealSense_None: 257 | { 258 | temporal_filtering_ = pcl::RealSenseGrabber::RealSense_Median; 259 | pcl::console::print_value ("median\n"); 260 | break; 261 | } 262 | case pcl::RealSenseGrabber::RealSense_Median: 263 | { 264 | temporal_filtering_ = pcl::RealSenseGrabber::RealSense_Average; 265 | pcl::console::print_value ("average\n"); 266 | break; 267 | } 268 | case pcl::RealSenseGrabber::RealSense_Average: 269 | { 270 | temporal_filtering_ = pcl::RealSenseGrabber::RealSense_None; 271 | pcl::console::print_value ("none\n"); 272 | break; 273 | } 274 | } 275 | grabber_.enableTemporalFiltering (temporal_filtering_, window_); 276 | } 277 | if (event.getKeyCode () == 'b') 278 | { 279 | with_bilateral_ = !with_bilateral_; 280 | pcl::console::print_info ("Bilateral filtering: "); 281 | pcl::console::print_value (with_bilateral_ ? "ON\n" : "OFF\n"); 282 | } 283 | if (event.getKeyCode () == 'a' || event.getKeyCode () == 'A') 284 | { 285 | float s = bilateral_.getSigmaS (); 286 | s += event.getKeyCode () == 'a' ? 1 : -1; 287 | if (s <= 1) 288 | s = 1; 289 | pcl::console::print_info ("Bilateral filter spatial sigma: "); 290 | pcl::console::print_value ("%.0f\n", s); 291 | bilateral_.setSigmaS (s); 292 | } 293 | if (event.getKeyCode () == 'z' || event.getKeyCode () == 'Z') 294 | { 295 | float r = bilateral_.getSigmaR (); 296 | r += event.getKeyCode () == 'z' ? 0.01 : -0.01; 297 | if (r <= 0.01) 298 | r = 0.01; 299 | pcl::console::print_info ("Bilateral filter range sigma: "); 300 | pcl::console::print_value ("%.2f\n", r); 301 | bilateral_.setSigmaR (r); 302 | } 303 | if (event.getKeyCode () == 's') 304 | { 305 | boost::format fmt ("RS_%s_%u.pcd"); 306 | std::string fn = boost::str (fmt % grabber_.getDeviceSerialNumber ().c_str () % last_cloud_->header.stamp); 307 | pcl::io::savePCDFileBinaryCompressed (fn, *last_cloud_); 308 | pcl::console::print_info ("Saved point cloud: "); 309 | pcl::console::print_value (fn.c_str ()); 310 | pcl::console::print_info ("\n"); 311 | } 312 | displaySettings (); 313 | } 314 | } 315 | 316 | void 317 | pointPickingCallback (const pcl::visualization::PointPickingEvent& event, void*) 318 | { 319 | float x, y, z; 320 | event.getPoint (x, y, z); 321 | pcl::console::print_info ("Picked point at "); 322 | pcl::console::print_value ("%.3f", x); pcl::console::print_info (", "); 323 | pcl::console::print_value ("%.3f", y); pcl::console::print_info (", "); 324 | pcl::console::print_value ("%.3f\n", z); 325 | } 326 | 327 | void 328 | displaySettings () 329 | { 330 | const int dx = 5; 331 | const int dy = 14; 332 | const int fs = 10; 333 | boost::format name_fmt ("text%i"); 334 | const char* TF[] = {"off", "median", "average"}; 335 | std::vector entries; 336 | // Framerate 337 | entries.push_back (boost::format ("framerate: %.1f") % grabber_.getFramesPerSecond ()); 338 | // Confidence threshold 339 | entries.push_back (boost::format ("confidence threshold: %i") % threshold_); 340 | // Temporal filter settings 341 | std::string tfs = boost::str (boost::format (", window size %i") % window_); 342 | entries.push_back (boost::format ("temporal filtering: %s%s") % TF[temporal_filtering_] % (temporal_filtering_ == pcl::RealSenseGrabber::RealSense_None ? "" : tfs)); 343 | // Bilateral filter settings 344 | std::string bfs = boost::str (boost::format ("spatial sigma %.0f, range sigma %.2f") % bilateral_.getSigmaS () % bilateral_.getSigmaR ()); 345 | entries.push_back (boost::format ("bilateral filtering: %s") % (with_bilateral_ ? bfs : "off")); 346 | for (size_t i = 0; i < entries.size (); ++i) 347 | { 348 | std::string name = boost::str (name_fmt % i); 349 | std::string entry = boost::str (entries[i]); 350 | if (!viewer_.updateText (entry, dx, dy + i * (fs + 2), fs, 1.0, 1.0, 1.0, name)) 351 | viewer_.addText (entry, dx, dy + i * (fs + 2), fs, 1.0, 1.0, 1.0, name); 352 | } 353 | } 354 | 355 | void 356 | printMode (const pcl::RealSenseGrabber::Mode& mode) 357 | { 358 | print_info ("Capturing mode: "); 359 | print_value ("%i", mode.fps); 360 | print_info (" Hz "); 361 | print_value ("%dx%d ", mode.depth_width, mode.depth_height); 362 | print_info ("Depth"); 363 | if (pcl::traits::has_color::value) 364 | { 365 | print_value (" %dx%d ", mode.color_width, mode.color_height); 366 | print_info ("Color"); 367 | } 368 | print_value ("\n"); 369 | } 370 | 371 | pcl::RealSenseGrabber& grabber_; 372 | pcl::visualization::PCLVisualizer viewer_; 373 | boost::signals2::connection connection_; 374 | 375 | pcl::FastBilateralFilter bilateral_; 376 | 377 | int window_; 378 | int threshold_; 379 | pcl::RealSenseGrabber::TemporalFilteringType temporal_filtering_; 380 | bool with_bilateral_; 381 | 382 | mutable boost::mutex new_cloud_mutex_; 383 | typename PointCloudT::ConstPtr new_cloud_; 384 | typename PointCloudT::ConstPtr last_cloud_; 385 | 386 | }; 387 | 388 | int 389 | main (int argc, char** argv) 390 | { 391 | print_info ("Viewer for RealSense devices (run with --help for more information)\n", argv[0]); 392 | 393 | if (find_switch (argc, argv, "--help") || find_switch (argc, argv, "-h")) 394 | { 395 | printHelp (argc, argv); 396 | return (0); 397 | } 398 | 399 | if (find_switch (argc, argv, "--list") || find_switch (argc, argv, "-l")) 400 | { 401 | printDeviceList (); 402 | return (0); 403 | } 404 | 405 | unsigned int mode_id = 0; 406 | bool with_mode = find_argument(argc, argv, "--mode") != -1; 407 | parse_argument(argc, argv, "--mode", mode_id); 408 | 409 | std::string device_id; 410 | 411 | if (argc == 1 || // no arguments 412 | (argc == 3 && with_mode)) // single argument, and it is --mode 413 | { 414 | device_id = ""; 415 | print_info ("Creating a grabber for the first available device\n"); 416 | } 417 | else 418 | { 419 | device_id = argv[argc - 1]; 420 | print_info ("Creating a grabber for device "); print_value ("%s\n", device_id.c_str ()); 421 | } 422 | 423 | try 424 | { 425 | pcl::RealSenseGrabber grabber (device_id); 426 | std::vector xyz_modes = grabber.getAvailableModes (true); 427 | std::vector xyzrgba_modes = grabber.getAvailableModes (false); 428 | if (mode_id == 0) 429 | { 430 | RealSenseViewer viewer (grabber); 431 | viewer.run (); 432 | } 433 | else if (mode_id <= xyz_modes.size ()) 434 | { 435 | grabber.setMode (xyz_modes[mode_id - 1], true); 436 | RealSenseViewer viewer (grabber); 437 | viewer.run (); 438 | } 439 | else if (mode_id <= xyz_modes.size () + xyzrgba_modes.size ()) 440 | { 441 | grabber.setMode (xyzrgba_modes[mode_id - xyz_modes.size () - 1], true); 442 | RealSenseViewer viewer (grabber); 443 | viewer.run (); 444 | } 445 | else 446 | { 447 | print_error ("Requested a mode (%i) that is not in the list of supported by this device\n", mode_id); 448 | return (1); 449 | } 450 | } 451 | catch (pcl::io::IOException& e) 452 | { 453 | print_error ("Failed to create a grabber: %s\n", e.what ()); 454 | return (1); 455 | } 456 | 457 | return (0); 458 | } 459 | 460 | -------------------------------------------------------------------------------- /src/tutorial.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | 3 | INTEL CORPORATION PROPRIETARY INFORMATION 4 | This software is supplied under the terms of a license agreement or nondisclosure 5 | agreement with Intel Corporation and may not be copied or disclosed except in 6 | accordance with the terms of that agreement 7 | Copyright(c) 2013-2014 Intel Corporation. All Rights Reserved. 8 | 9 | *******************************************************************************/ 10 | /* 11 | Description: 12 | This is the raw streams procedural sample that shows how to capture color and depth synchronized by procedural calls. 13 | 14 | */ 15 | 16 | #include 17 | #include 18 | #include 19 | //#include "util_render.h" //SDK provided utility class used for rendering (packaged in libpxcutils.lib) 20 | 21 | // maximum number of frames to process if user did not close the rendering window 22 | #define MAX_FRAMES 5000 23 | 24 | int wmain(int argc, WCHAR* argv[]) { 25 | 26 | // initialize the util render 27 | //UtilRender *renderColor = new UtilRender(L"COLOR STREAM"); 28 | //UtilRender *renderDepth = new UtilRender(L"DEPTH STREAM"); 29 | 30 | // create the PXCSenseManager 31 | PXCSenseManager *psm=0; 32 | psm = PXCSenseManager::CreateInstance(); 33 | if (!psm) { 34 | wprintf_s(L"Unable to create the PXCSenseManager\n"); 35 | return 1; 36 | } 37 | 38 | // select the color stream of size 640x480 and depth stream of size 320x240 39 | psm->EnableStream(PXCCapture::STREAM_TYPE_COLOR, 640, 480); 40 | psm->EnableStream(PXCCapture::STREAM_TYPE_DEPTH, 640, 480); 41 | 42 | // initialize the PXCSenseManager 43 | if(psm->Init() != PXC_STATUS_NO_ERROR) return 2; 44 | 45 | PXCImage *colorIm, *depthIm; 46 | for (int i=0; iAcquireFrame(true)QuerySample(); 54 | 55 | // retrieve the image or frame by type from the sample 56 | colorIm = sample->color; 57 | depthIm = sample->depth; 58 | 59 | // render the frame 60 | //if (!renderColor->RenderFrame(colorIm)) break; 61 | //if (!renderDepth->RenderFrame(depthIm)) break; 62 | 63 | // release or unlock the current frame to fetch the next frame 64 | psm->ReleaseFrame(); 65 | } 66 | 67 | // delete the UtilRender instance 68 | //delete renderColor; 69 | //delete renderDepth; 70 | 71 | // close the last opened streams and release any session and processing module instances 72 | psm->Release(); 73 | 74 | return 0; 75 | } --------------------------------------------------------------------------------