From d20fa4e35e759449d949a9014bf30ed60d6471a0 Mon Sep 17 00:00:00 2001 From: lebronzhang Date: Tue, 12 Jul 2016 01:55:12 +0800 Subject: [PATCH 1/3] Add librealsense support to RealSenseGrabber --- CMakeLists.txt | 1 + cmake/Modules/Findlibrealsense.cmake | 31 ++ io/CMakeLists.txt | 24 +- .../librealsense/real_sense_device_manager.h | 164 +++++++++ .../{ => sdk}/real_sense_device_manager.h | 0 io/include/pcl/io/real_sense_grabber.h | 1 + .../real_sense_device_manager.cpp | 117 +++++++ .../librealsense/real_sense_grabber.cpp | 310 +++++++++++++++++ .../{ => sdk}/real_sense_device_manager.cpp | 2 +- io/src/real_sense/sdk/real_sense_grabber.cpp | 315 ++++++++++++++++++ io/src/real_sense_grabber.cpp | 274 +-------------- visualization/CMakeLists.txt | 5 +- visualization/tools/CMakeLists.txt | 4 +- 13 files changed, 967 insertions(+), 281 deletions(-) create mode 100644 cmake/Modules/Findlibrealsense.cmake create mode 100644 io/include/pcl/io/real_sense/librealsense/real_sense_device_manager.h rename io/include/pcl/io/real_sense/{ => sdk}/real_sense_device_manager.h (100%) create mode 100644 io/src/real_sense/librealsense/real_sense_device_manager.cpp create mode 100644 io/src/real_sense/librealsense/real_sense_grabber.cpp rename io/src/real_sense/{ => sdk}/real_sense_device_manager.cpp (99%) create mode 100644 io/src/real_sense/sdk/real_sense_grabber.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e5fd763e31f..fd9e658e878 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -299,6 +299,7 @@ PCL_ADD_GRABBER_DEPENDENCY("Ensenso" "IDS-Imaging Ensenso camera support") PCL_ADD_GRABBER_DEPENDENCY("davidSDK" "David Vision Systems SDK support") PCL_ADD_GRABBER_DEPENDENCY("DSSDK" "DepthSense SDK support") PCL_ADD_GRABBER_DEPENDENCY("RSSDK" "RealSense SDK support") +PCL_ADD_GRABBER_DEPENDENCY("librealsense" "Intel librealsense support") # metslib if (PKG_CONFIG_FOUND) diff --git a/cmake/Modules/Findlibrealsense.cmake b/cmake/Modules/Findlibrealsense.cmake new file mode 100644 index 00000000000..82bcdc0387c --- /dev/null +++ b/cmake/Modules/Findlibrealsense.cmake @@ -0,0 +1,31 @@ +############################################################################### +# Find librealsense +# +# find_package(librealsense) +# +# Variables defined by this module: +# +# LIBREALSENSE_FOUND True if librealsense was found +# LIBREALSENSE_INCLUDE_DIRS The location(s) of librealsense headers +# LIBREALSENSE_LIBRARIES Libraries needed to use librealsense + +if (LIBREALSENSE_LIBRARIES AND LIBREALSENSE_INCLUDE_DIRS) + # in cache already + set(LIBREALSENSE_FOUND TRUE) +else (LIBREALSENSE_LIBRARIES AND LIBREALSENSE_INCLUDE_DIRS) +find_path(LIBREALSENSE_INCLUDE_DIR + NAMES librealsense + PATHS /usr/local/include /usr/include /opt/local/include /sw/include) + +find_library(LIBREALSENSE_LIBRARY + NAMES librealsense.so + PATHS /usr/local/lib /usr/lib /opt/local/lib /sw/lib) + +set(LIBREALSENSE_INCLUDE_DIRS ${LIBREALSENSE_INCLUDE_DIR}) +set(LIBREALSENSE_LIBRARIES ${LIBREALSENSE_LIBRARY}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(LIBREALSENSE LIBREALSENSE_LIBRARY LIBREALSENSE_INCLUDE_DIR) + +mark_as_advanced(LIBREALSENSE_INCLUDE_DIRS LIBREALSENSE_LIBRARIES) +endif (LIBREALSENSE_LIBRARIES AND LIBREALSENSE_INCLUDE_DIRS) diff --git a/io/CMakeLists.txt b/io/CMakeLists.txt index c10be950b11..70e8c96ed61 100644 --- a/io/CMakeLists.txt +++ b/io/CMakeLists.txt @@ -7,7 +7,7 @@ PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) if(WIN32) PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni openni2 ensenso davidSDK dssdk rssdk pcap png vtk) else(WIN32) - PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni openni2 ensenso davidSDK dssdk pcap png vtk libusb-1.0) + PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni openni2 ensenso davidSDK dssdk pcap png vtk libusb-1.0 librealsense) endif(WIN32) PCL_ADD_DOC("${SUBSYS_NAME}") @@ -151,13 +151,23 @@ if(build) endif() if(WITH_RSSDK) - set(RSSDK_GRABBER_INCLUDES + set(REALSENSE_GRABBER_INCLUDES include/pcl/io/real_sense_grabber.h ) - set(RSSDK_GRABBER_SOURCES + set(REALSENSE_GRABBER_SOURCES src/real_sense_grabber.cpp - src/real_sense/real_sense_device_manager.cpp + src/real_sense/sdk/real_sense_grabber.cpp + src/real_sense/sdk/real_sense_device_manager.cpp ) + elseif(WITH_LIBREALSENSE) + set(REALSENSE_GRABBER_INCLUDES + include/pcl/io/real_sense_grabber.h + ) + set(REALSENSE_GRABBER_SOURCES + src/real_sense_grabber.cpp + src/real_sense/librealsense/real_sense_grabber.cpp + src/real_sense/librealsense/real_sense_device_manager.cpp + ) endif() if(LIBUSB_1_FOUND) @@ -226,7 +236,7 @@ if(build) ${ENSENSO_GRABBER_SOURCES} ${DAVIDSDK_GRABBER_SOURCES} ${DSSDK_GRABBER_SOURCES} - ${RSSDK_GRABBER_SOURCES} + ${REALSENSE_GRABBER_SOURCES} ) if(PNG_FOUND) list(APPEND srcs @@ -274,7 +284,7 @@ if(build) ${ENSENSO_GRABBER_INCLUDES} ${DAVIDSDK_GRABBER_INCLUDES} ${DSSDK_GRABBER_INCLUDES} - ${RSSDK_GRABBER_INCLUDES} + ${REALSENSE_GRABBER_INCLUDES} "include/pcl/${SUBSYS_NAME}/pxc_grabber.h" # contains only depreciation note ) @@ -357,6 +367,8 @@ if(build) if(WITH_RSSDK) target_link_libraries(${LIB_NAME} ${RSSDK_LIBRARIES}) + elseif(WITH_LIBREALSENSE) + target_link_libraries(${LIB_NAME} ${LIBREALSENSE_LIBRARIES}) endif() if (PCAP_FOUND) diff --git a/io/include/pcl/io/real_sense/librealsense/real_sense_device_manager.h b/io/include/pcl/io/real_sense/librealsense/real_sense_device_manager.h new file mode 100644 index 00000000000..1e98a43ecb4 --- /dev/null +++ b/io/include/pcl/io/real_sense/librealsense/real_sense_device_manager.h @@ -0,0 +1,164 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2015-, Open Perception, Inc. + * Copyright (c) 2016, Intel Corporation + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_IO_REAL_SENSE_DEVICE_MANAGER_H +#define PCL_IO_REAL_SENSE_DEVICE_MANAGER_H + +#include +#include +#include +#include +#include + +#include + +#include + +namespace pcl +{ + + class RealSenseGrabber; + + namespace io + { + + namespace real_sense + { + + class RealSenseDevice; + + class PCL_EXPORTS RealSenseDeviceManager : boost::noncopyable + { + + public: + + typedef boost::shared_ptr Ptr; + + static Ptr& + getInstance () + { + static Ptr instance; + if (!instance) + { + boost::mutex::scoped_lock lock (mutex_); + if (!instance) + instance.reset (new RealSenseDeviceManager); + } + return (instance); + } + + inline size_t + getNumDevices () + { + return (device_list_.size ()); + } + + boost::shared_ptr + captureDevice (); + + boost::shared_ptr + captureDevice (size_t index); + + boost::shared_ptr + captureDevice (const std::string& sn); + + ~RealSenseDeviceManager (); + + private: + + struct DeviceInfo + { + int index; + std::string serial; + bool isCaptured; + }; + + /** If the device is already captured returns a pointer. */ + boost::shared_ptr + capture (DeviceInfo& device_info); + + RealSenseDeviceManager (); + + rs::context context_; + + /** This function discovers devices that are capable of streaming + * depth data. */ + void + populateDeviceList (); + + std::vector device_list_; + + static boost::mutex mutex_; + }; + + class PCL_EXPORTS RealSenseDevice : boost::noncopyable + { + public: + + typedef boost::shared_ptr Ptr; + + inline const std::string& + getSerialNumber () { return (device_id_); } + + inline rs::device* + getDevice () { return (device_); } + + /** Reset the state of given device by releasing and capturing again. */ + static void + reset (RealSenseDevice::Ptr& device) + { + std::string id = device->getSerialNumber (); + device.reset (); + device = RealSenseDeviceManager::getInstance ()->captureDevice (id); + } + + private: + + friend class RealSenseDeviceManager; + + std::string device_id_; + rs::device* device_; + + RealSenseDevice (const std::string& id) : device_id_ (id) { }; + + }; + } // namespace real_sense + } // namespace io +} // namespace pcl + +#endif /* PCL_IO_REAL_SENSE_DEVICE_MANAGER_H */ \ No newline at end of file diff --git a/io/include/pcl/io/real_sense/real_sense_device_manager.h b/io/include/pcl/io/real_sense/sdk/real_sense_device_manager.h similarity index 100% rename from io/include/pcl/io/real_sense/real_sense_device_manager.h rename to io/include/pcl/io/real_sense/sdk/real_sense_device_manager.h diff --git a/io/include/pcl/io/real_sense_grabber.h b/io/include/pcl/io/real_sense_grabber.h index 4ddf3f06af7..6551fa0adf3 100644 --- a/io/include/pcl/io/real_sense_grabber.h +++ b/io/include/pcl/io/real_sense_grabber.h @@ -3,6 +3,7 @@ * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2015-, Open Perception, Inc. + * Copyright (c) 2016, Intel Corporation * * All rights reserved. * diff --git a/io/src/real_sense/librealsense/real_sense_device_manager.cpp b/io/src/real_sense/librealsense/real_sense_device_manager.cpp new file mode 100644 index 00000000000..37b95b59b2c --- /dev/null +++ b/io/src/real_sense/librealsense/real_sense_device_manager.cpp @@ -0,0 +1,117 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2015-, Open Perception, Inc. + * Copyright (c) 2016, Intel Corporation + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ +#include +#include + +boost::mutex pcl::io::real_sense::RealSenseDeviceManager::mutex_; + +using namespace pcl::io; + +pcl::io::real_sense::RealSenseDeviceManager::RealSenseDeviceManager () +{ + populateDeviceList (); +} + +pcl::io::real_sense::RealSenseDeviceManager::~RealSenseDeviceManager () +{ +} + +pcl::io::real_sense::RealSenseDevice::Ptr +pcl::io::real_sense::RealSenseDeviceManager::captureDevice () +{ + boost::mutex::scoped_lock lock (mutex_); + if (device_list_.size () == 0) + THROW_IO_EXCEPTION ("no connected devices"); + for (size_t i = 0; i < device_list_.size (); ++i) + if (!device_list_[i].isCaptured) + return (capture (device_list_[i])); + THROW_IO_EXCEPTION ("all connected devices are captured by other grabbers"); + return (RealSenseDevice::Ptr ()); // never reached, needed just to silence -Wreturn-type warning +} + +pcl::io::real_sense::RealSenseDevice::Ptr +pcl::io::real_sense::RealSenseDeviceManager::captureDevice (size_t index) +{ + boost::mutex::scoped_lock lock (mutex_); + if (index >= device_list_.size ()) + THROW_IO_EXCEPTION ("device with index %i is not connected", index + 1); + if (device_list_[index].isCaptured) + THROW_IO_EXCEPTION ("device with index %i is captured by another grabber", index + 1); + return (capture (device_list_[index])); +} + +pcl::io::real_sense::RealSenseDevice::Ptr +pcl::io::real_sense::RealSenseDeviceManager::captureDevice (const std::string& sn) +{ + boost::mutex::scoped_lock lock (mutex_); + for (size_t i = 0; i < device_list_.size (); ++i) + { + if (device_list_[i].serial == sn) + { + if (device_list_[i].isCaptured) + THROW_IO_EXCEPTION ("device with serial number %s is captured by another grabber", sn.c_str ()); + return (capture (device_list_[i])); + } + } + THROW_IO_EXCEPTION ("device with serial number %s is not connected", sn.c_str ()); + return (RealSenseDevice::Ptr ()); // never reached, needed just to silence -Wreturn-type warning +} + +void +pcl::io::real_sense::RealSenseDeviceManager::populateDeviceList () +{ + device_list_.clear (); + for (int i = 0; i < context_.get_device_count (); i++) + { + device_list_.push_back (DeviceInfo ()); + device_list_.back ().serial = context_.get_device (i)->get_serial (); + device_list_.back ().index = i; + device_list_.back ().isCaptured = false; + } +} + +pcl::io::real_sense::RealSenseDevice::Ptr +pcl::io::real_sense::RealSenseDeviceManager::capture (DeviceInfo& device_info) +{ + // This is called from public captureDevice() functions and should already be + // under scoped lock + RealSenseDevice::Ptr device (new RealSenseDevice (device_info.serial)); + device->device_ = context_.get_device (device_info.index); + device_info.isCaptured = true; + return device; +} \ No newline at end of file diff --git a/io/src/real_sense/librealsense/real_sense_grabber.cpp b/io/src/real_sense/librealsense/real_sense_grabber.cpp new file mode 100644 index 00000000000..2eb92ec4b06 --- /dev/null +++ b/io/src/real_sense/librealsense/real_sense_grabber.cpp @@ -0,0 +1,310 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2015-, Open Perception, Inc. + * Copyright (c) 2016, Intel Corporation + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include + +#include + +#include +#include +#include +#include + + +using namespace pcl::io; +using namespace pcl::io::real_sense; + +pcl::RealSenseGrabber::RealSenseGrabber (const std::string& device_id, const Mode& mode, bool strict) +: Grabber () +, is_running_ (false) +, confidence_threshold_ (6) +, temporal_filtering_type_ (RealSense_None) +, temporal_filtering_window_size_ (1) +, mode_requested_ (mode) +, strict_ (strict) +{ + if (device_id == "") + device_ = RealSenseDeviceManager::getInstance ()->captureDevice (); + else if (device_id[0] == '#') + device_ = RealSenseDeviceManager::getInstance ()->captureDevice (boost::lexical_cast (device_id.substr (1)) - 1); + else + device_ = RealSenseDeviceManager::getInstance ()->captureDevice (device_id); + + point_cloud_signal_ = createSignal (); + point_cloud_rgba_signal_ = createSignal (); +} + +void +pcl::RealSenseGrabber::start () +{ + if (!is_running_) + { + need_xyz_ = num_slots () > 0; + need_xyzrgba_ = num_slots () > 0; + if (need_xyz_ || need_xyzrgba_) + { + if (temporal_filtering_type_ != RealSense_None) + { + temporal_filtering_type_ = RealSense_None; + PCL_WARN ("[pcl::RealSenseGrabber::enableTemporalFiltering] This device does not support temporal filter with librealsense now\n"); + } + selectMode (); + device_->getDevice ()->enable_stream (rs::stream::depth, mode_selected_.depth_width, mode_selected_.depth_height, rs::format::z16, mode_selected_.fps); + } + if (need_xyzrgba_) + { + device_->getDevice ()->enable_stream (rs::stream::color, mode_selected_.color_width, mode_selected_.color_height, rs::format::rgb8, mode_selected_.fps); + } + frequency_.reset (); + is_running_ = true; + thread_ = boost::thread (&RealSenseGrabber::run, this); + } +} + +const std::string& +pcl::RealSenseGrabber::getDeviceSerialNumber () const +{ + return (device_->getSerialNumber ()); +} + +void +pcl::RealSenseGrabber::setConfidenceThreshold (unsigned int threshold) +{ + if (threshold > 15) + { + PCL_WARN ("[pcl::RealSenseGrabber::setConfidenceThreshold] Attempted to set threshold outside valid range (0-15)"); + } + else if (!device_->getDevice ()->supports_option (rs::option::f200_confidence_threshold)) + { + PCL_WARN ("[pcl::RealSenseGrabber::setConfidenceThreshold] This device does not support setting confidence threshold with librealsense now\n"); + } + else + { + device_->getDevice ()->set_option (rs::option::f200_confidence_threshold, threshold); + } +} + +std::vector +pcl::RealSenseGrabber::getAvailableModes (bool only_depth) const +{ + std::vector modes; + int depth_total = device_->getDevice ()->get_stream_mode_count (rs::stream::depth); + int color_total = device_->getDevice ()->get_stream_mode_count (rs::stream::color); + int depth_width = 0, depth_height = 0, depth_fps = 0, depth_width_old = 0, depth_fps_old = 0, + color_width = 0, color_height = 0, color_fps = 0, color_width_old = 0, color_fps_old = 0; + rs::format format; + for (int i = 0; i < depth_total; i++) + { + device_->getDevice ()->get_stream_mode (rs::stream::depth, i, depth_width, depth_height, format, depth_fps); + if ( depth_width == depth_width_old && depth_fps == depth_fps_old) continue; + depth_width_old = depth_width; + depth_fps_old = depth_fps; + if (!only_depth) + { + for (int i = 0; i < color_total; i++) + { + device_->getDevice ()->get_stream_mode (rs::stream::color, i, color_width, color_height, format, color_fps); + if ((color_width == color_width_old && color_fps == color_fps_old) || (depth_fps != color_fps)) continue; + color_width_old = color_width; + color_fps_old = color_fps; + Mode mode; + mode.fps = depth_fps; + mode.depth_width = depth_width; + mode.depth_height = depth_height; + mode.color_width = color_width; + mode.color_height = color_height; + modes.push_back (mode); + } + } + else + { + Mode mode; + mode.fps = depth_fps; + mode.depth_width = depth_width; + mode.depth_height = depth_height; + modes.push_back (mode); + } + } + return modes; +} + +void +pcl::RealSenseGrabber::run () +{ + rs::device* device = device_->getDevice (); + device->start (); + int depth_width = mode_selected_.depth_width, depth_height = mode_selected_.depth_height, + depth_size = depth_width * depth_height, color_width = mode_selected_.color_width, + color_height = mode_selected_.color_height, color_size = color_width * color_height * 3; + ///Buffer to store depth data + std::vector depth_data; + ///Buffer to store color data + std::vector color_data; + while (is_running_) + { + if (device->is_streaming ()) device->wait_for_frames (); + fps_mutex_.lock (); + frequency_.event (); + fps_mutex_.unlock (); + // Retrieve our images + const uint16_t * depth_image = (const uint16_t *)device->get_frame_data (rs::stream::depth); + const uint8_t * color_image = (const uint8_t *)device->get_frame_data (rs::stream::color); + // Retrieve camera parameters for mapping between depth and color + rs::intrinsics depth_intrin = device->get_stream_intrinsics (rs::stream::depth); + rs::intrinsics color_intrin = device->get_stream_intrinsics (rs::stream::color); + rs::extrinsics depth_to_color = device->get_extrinsics (rs::stream::depth, rs::stream::color); + float scale = device->get_depth_scale (); + depth_data.clear (); + depth_data.resize (depth_size); + pcl::PointCloud::Ptr xyz_cloud; + pcl::PointCloud::Ptr xyzrgba_cloud; + float max_distance = 6; // get rid of noisy data that is past 6 meters + static const float nan = std::numeric_limits::quiet_NaN (); + + memcpy (depth_data.data (), &depth_image[0], depth_size * sizeof (uint16_t)); + + if (need_xyzrgba_) + { + color_data.clear (); + color_data.resize (color_size); + memcpy (color_data.data (), &color_image[0], color_size * sizeof (uint8_t)); + xyzrgba_cloud.reset (new pcl::PointCloud (depth_width, depth_height)); + xyzrgba_cloud->is_dense = false; + if (need_xyz_) + { + xyz_cloud.reset (new pcl::PointCloud (depth_width, depth_height)); + xyz_cloud->is_dense = false; + } + for (int dy = 0; dy < depth_height; ++dy) + { + uint32_t i = dy * depth_width - 1; + for (int dx = 0; dx < depth_width; ++dx) + { + i++; + // Retrieve the 16-bit depth value and map it into a depth in meters + uint16_t depth_value = depth_data[i]; + float depth_in_meters = depth_value * scale; + + // Map from pixel coordinates in the depth image to real world co-ordinates + rs::float2 depth_pixel = {(float)dx, (float)dy}; + rs::float3 depth_point = depth_intrin.deproject (depth_pixel, depth_in_meters); + rs::float3 color_point = depth_to_color.transform (depth_point); + rs::float2 color_pixel = color_intrin.project (color_point); + + const int cx = (int)std::round (color_pixel.x), cy = (int)std::round (color_pixel.y); + int red = 0, green = 0, blue = 0; + if (cx < 0 || cy < 0 || cx >= color_width || cy >= color_height) + { + red = 255; green = 255; blue = 255; + } + else + { + int pos = (cy * color_width + cx) * 3; + red = color_data[pos]; + green = color_data[pos + 1]; + blue = color_data[pos + 2]; + } + if (depth_value == 0 || depth_point.z > max_distance) + { + xyzrgba_cloud->points[i].x = xyzrgba_cloud->points[i].y = xyzrgba_cloud->points[i].z = (float) nan; + if (need_xyz_) + { + xyz_cloud->points[i].x = xyz_cloud->points[i].y = xyz_cloud->points[i].z = (float) nan; + } + continue; + } + else + { + xyzrgba_cloud->points[i].x = -depth_point.x; + xyzrgba_cloud->points[i].y = -depth_point.y; + xyzrgba_cloud->points[i].z = depth_point.z; + xyzrgba_cloud->points[i].r = red; + xyzrgba_cloud->points[i].g = green; + xyzrgba_cloud->points[i].b = blue; + if (need_xyz_) + { + xyz_cloud->points[i].x = -depth_point.x; + xyz_cloud->points[i].y = -depth_point.y; + xyz_cloud->points[i].z = depth_point.z; + } + } + } + } + point_cloud_rgba_signal_->operator () (xyzrgba_cloud); + if (need_xyz_) + { + point_cloud_signal_->operator () (xyz_cloud); + } + } + else if (need_xyz_) + { + xyz_cloud.reset (new pcl::PointCloud (depth_width, depth_height)); + xyz_cloud->is_dense = false; + for (int dy = 0; dy < depth_height; ++dy) + { + uint32_t i = dy * depth_width - 1; + for (int dx = 0; dx < depth_width; ++dx) + { + i++; + // Retrieve the 16-bit depth value and map it into a depth in meters + uint16_t depth_value = depth_data[i]; + float depth_in_meters = depth_value * scale; + rs::float2 depth_pixel = {(float)dx, (float)dy}; + rs::float3 depth_point = depth_intrin.deproject (depth_pixel, depth_in_meters); + if (depth_value == 0 || depth_point.z > max_distance) + { + xyz_cloud->points[i].x = xyz_cloud->points[i].y = xyz_cloud->points[i].z = (float) nan; + continue; + } + else + { + xyz_cloud->points[i].x = -depth_point.x; + xyz_cloud->points[i].y = -depth_point.y; + xyz_cloud->points[i].z = depth_point.z; + } + } + } + point_cloud_signal_->operator () (xyz_cloud); + } + else + { + //do nothing + } + } + device->stop (); +} \ No newline at end of file diff --git a/io/src/real_sense/real_sense_device_manager.cpp b/io/src/real_sense/sdk/real_sense_device_manager.cpp similarity index 99% rename from io/src/real_sense/real_sense_device_manager.cpp rename to io/src/real_sense/sdk/real_sense_device_manager.cpp index 2a1557ca570..63f838104e3 100644 --- a/io/src/real_sense/real_sense_device_manager.cpp +++ b/io/src/real_sense/sdk/real_sense_device_manager.cpp @@ -37,7 +37,7 @@ #include -#include +#include boost::mutex pcl::io::real_sense::RealSenseDeviceManager::mutex_; diff --git a/io/src/real_sense/sdk/real_sense_grabber.cpp b/io/src/real_sense/sdk/real_sense_grabber.cpp new file mode 100644 index 00000000000..3a0a55032bf --- /dev/null +++ b/io/src/real_sense/sdk/real_sense_grabber.cpp @@ -0,0 +1,315 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2015-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace pcl::io; +using namespace pcl::io::real_sense; + +/* Helper function to convert a PXCPoint3DF32 point into a PCL point. + * Takes care of unit conversion (PXC point coordinates are in millimeters) + * and invalid points. */ +template inline void +convertPoint (const PXCPoint3DF32& src, T& tgt) +{ + static const float nan = std::numeric_limits::quiet_NaN (); + if (src.z == 0) + { + tgt.x = tgt.y = tgt.z = nan; + } + else + { + tgt.x = -src.x / 1000.0; + tgt.y = src.y / 1000.0; + tgt.z = src.z / 1000.0; + } +} + +pcl::RealSenseGrabber::RealSenseGrabber (const std::string& device_id, const Mode& mode, bool strict) +: Grabber () +, is_running_ (false) +, confidence_threshold_ (6) +, temporal_filtering_type_ (RealSense_None) +, temporal_filtering_window_size_ (1) +, mode_requested_ (mode) +, strict_ (strict) +{ + if (device_id == "") + device_ = RealSenseDeviceManager::getInstance ()->captureDevice (); + else if (device_id[0] == '#') + device_ = RealSenseDeviceManager::getInstance ()->captureDevice (boost::lexical_cast (device_id.substr (1)) - 1); + else + device_ = RealSenseDeviceManager::getInstance ()->captureDevice (device_id); + + point_cloud_signal_ = createSignal (); + point_cloud_rgba_signal_ = createSignal (); +} + +void +pcl::RealSenseGrabber::start () +{ + if (!is_running_) + { + need_xyz_ = num_slots () > 0; + need_xyzrgba_ = num_slots () > 0; + if (need_xyz_ || need_xyzrgba_) + { + selectMode (); + PXCCapture::Device::StreamProfileSet profile; + memset (&profile, 0, sizeof (profile)); + profile.depth.frameRate.max = mode_selected_.fps; + profile.depth.frameRate.min = mode_selected_.fps; + profile.depth.imageInfo.width = mode_selected_.depth_width; + profile.depth.imageInfo.height = mode_selected_.depth_height; + profile.depth.imageInfo.format = PXCImage::PIXEL_FORMAT_DEPTH; + profile.depth.options = PXCCapture::Device::STREAM_OPTION_ANY; + if (need_xyzrgba_) + { + profile.color.frameRate.max = mode_selected_.fps; + profile.color.frameRate.min = mode_selected_.fps; + profile.color.imageInfo.width = mode_selected_.color_width; + profile.color.imageInfo.height = mode_selected_.color_height; + profile.color.imageInfo.format = PXCImage::PIXEL_FORMAT_RGB32; + profile.color.options = PXCCapture::Device::STREAM_OPTION_ANY; + } + device_->getPXCDevice ().SetStreamProfileSet (&profile); + if (!device_->getPXCDevice ().IsStreamProfileSetValid (&profile)) + THROW_IO_EXCEPTION ("Invalid stream profile for PXC device"); + frequency_.reset (); + is_running_ = true; + thread_ = boost::thread (&RealSenseGrabber::run, this); + } + } +} + +const std::string& +pcl::RealSenseGrabber::getDeviceSerialNumber () const +{ + return (device_->getSerialNumber ()); +} + +void +pcl::RealSenseGrabber::setConfidenceThreshold (unsigned int threshold) +{ + if (threshold > 15) + { + PCL_WARN ("[pcl::RealSenseGrabber::setConfidenceThreshold] Attempted to set threshold outside valid range (0-15)"); + } + else + { + confidence_threshold_ = threshold; + device_->getPXCDevice ().SetDepthConfidenceThreshold (confidence_threshold_); + } +} + +std::vector +pcl::RealSenseGrabber::getAvailableModes (bool only_depth) const +{ + std::vector modes; + PXCCapture::StreamType streams = only_depth + ? PXCCapture::STREAM_TYPE_DEPTH + : PXCCapture::STREAM_TYPE_DEPTH | PXCCapture::STREAM_TYPE_COLOR; + for (int p = 0;; p++) + { + PXCCapture::Device::StreamProfileSet profiles = {}; + if (device_->getPXCDevice ().QueryStreamProfileSet (streams, p, &profiles) == PXC_STATUS_NO_ERROR) + { + if (!only_depth && profiles.depth.frameRate.max != profiles.color.frameRate.max) + continue; // we need both streams to have the same framerate + Mode mode; + mode.fps = profiles.depth.frameRate.max; + mode.depth_width = profiles.depth.imageInfo.width; + mode.depth_height = profiles.depth.imageInfo.height; + mode.color_width = profiles.color.imageInfo.width; + mode.color_height = profiles.color.imageInfo.height; + bool duplicate = false; + for (size_t i = 0; i < modes.size (); ++i) + duplicate |= modes[i] == mode; + if (!duplicate) + modes.push_back (mode); + } + else + { + break; + } + } + return modes; +} + +void +pcl::RealSenseGrabber::run () +{ + const int WIDTH = mode_selected_.depth_width; + const int HEIGHT = mode_selected_.depth_height; + const int SIZE = WIDTH * HEIGHT; + + PXCProjection* projection = device_->getPXCDevice ().CreateProjection (); + PXCCapture::Sample sample; + std::vector vertices (SIZE); + createDepthBuffer (); + + while (is_running_) + { + pcl::PointCloud::Ptr xyz_cloud; + pcl::PointCloud::Ptr xyzrgba_cloud; + + pxcStatus status; + if (need_xyzrgba_) + status = device_->getPXCDevice ().ReadStreams (PXCCapture::STREAM_TYPE_DEPTH | PXCCapture::STREAM_TYPE_COLOR, &sample); + else + status = device_->getPXCDevice ().ReadStreams (PXCCapture::STREAM_TYPE_DEPTH, &sample); + + uint64_t timestamp = pcl::getTime () * 1.0e+6; + + switch (status) + { + case PXC_STATUS_NO_ERROR: + { + fps_mutex_.lock (); + frequency_.event (); + fps_mutex_.unlock (); + + /* We preform the following steps to convert received data into point clouds: + * + * 1. Push depth image to the depth buffer + * 2. Pull filtered depth image from the depth buffer + * 3. Project (filtered) depth image into 3D + * 4. Fill XYZ point cloud with computed points + * 5. Fill XYZRGBA point cloud with computed points + * 7. Project color image into 3D + * 6. Assign colors to points in XYZRGBA point cloud + * + * Steps 1-2 are skipped if temporal filtering is disabled. + * Step 4 is skipped if there are no subscribers for XYZ clouds. + * Steps 5-7 are skipped if there are no subscribers for XYZRGBA clouds. */ + + if (temporal_filtering_type_ != RealSense_None) + { + PXCImage::ImageData data; + sample.depth->AcquireAccess (PXCImage::ACCESS_READ, &data); + std::vector data_copy (SIZE); + memcpy (data_copy.data (), data.planes[0], SIZE * sizeof (unsigned short)); + sample.depth->ReleaseAccess (&data); + + depth_buffer_->push (data_copy); + + sample.depth->AcquireAccess (PXCImage::ACCESS_WRITE, &data); + unsigned short* d = reinterpret_cast (data.planes[0]); + for (size_t i = 0; i < SIZE; i++) + d[i] = (*depth_buffer_)[i]; + sample.depth->ReleaseAccess (&data); + } + + projection->QueryVertices (sample.depth, vertices.data ()); + + if (need_xyz_) + { + xyz_cloud.reset (new pcl::PointCloud (WIDTH, HEIGHT)); + xyz_cloud->header.stamp = timestamp; + xyz_cloud->is_dense = false; + for (int i = 0; i < SIZE; i++) + convertPoint (vertices[i], xyz_cloud->points[i]); + } + + if (need_xyzrgba_) + { + PXCImage::ImageData data; + PXCImage* mapped = projection->CreateColorImageMappedToDepth (sample.depth, sample.color); + mapped->AcquireAccess (PXCImage::ACCESS_READ, &data); + uint32_t* d = reinterpret_cast (data.planes[0]); + if (need_xyz_) + { + // We can fill XYZ coordinates more efficiently using pcl::copyPointCloud, + // given that they were already computed for XYZ point cloud. + xyzrgba_cloud.reset (new pcl::PointCloud); + pcl::copyPointCloud (*xyz_cloud, *xyzrgba_cloud); + for (int i = 0; i < HEIGHT; i++) + { + pcl::PointXYZRGBA* cloud_row = &xyzrgba_cloud->points[i * WIDTH]; + uint32_t* color_row = &d[i * data.pitches[0] / sizeof (uint32_t)]; + for (int j = 0; j < WIDTH; j++) + memcpy (&cloud_row[j].rgba, &color_row[j], sizeof (uint32_t)); + } + } + else + { + xyzrgba_cloud.reset (new pcl::PointCloud (WIDTH, HEIGHT)); + xyzrgba_cloud->header.stamp = timestamp; + xyzrgba_cloud->is_dense = false; + for (int i = 0; i < HEIGHT; i++) + { + PXCPoint3DF32* vertices_row = &vertices[i * WIDTH]; + pcl::PointXYZRGBA* cloud_row = &xyzrgba_cloud->points[i * WIDTH]; + uint32_t* color_row = &d[i * data.pitches[0] / sizeof (uint32_t)]; + for (int j = 0; j < WIDTH; j++) + { + convertPoint (vertices_row[j], cloud_row[j]); + memcpy (&cloud_row[j].rgba, &color_row[j], sizeof (uint32_t)); + } + } + } + mapped->ReleaseAccess (&data); + mapped->Release (); + } + + if (need_xyzrgba_) + point_cloud_rgba_signal_->operator () (xyzrgba_cloud); + if (need_xyz_) + point_cloud_signal_->operator () (xyz_cloud); + break; + } + case PXC_STATUS_DEVICE_LOST: + THROW_IO_EXCEPTION ("failed to read data stream from PXC device: device lost"); + case PXC_STATUS_ALLOC_FAILED: + THROW_IO_EXCEPTION ("failed to read data stream from PXC device: alloc failed"); + } + sample.ReleaseImages (); + } + projection->Release (); + RealSenseDevice::reset (device_); +} \ No newline at end of file diff --git a/io/src/real_sense_grabber.cpp b/io/src/real_sense_grabber.cpp index 14a26882ca5..440b2f94143 100644 --- a/io/src/real_sense_grabber.cpp +++ b/io/src/real_sense_grabber.cpp @@ -3,6 +3,7 @@ * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2015-, Open Perception, Inc. + * Copyright (c) 2016, Intel Corporation * * All rights reserved. * @@ -35,12 +36,6 @@ * */ -#include - -#include -#include -#include -#include #include #include @@ -48,30 +43,10 @@ #include #include #include -#include using namespace pcl::io; using namespace pcl::io::real_sense; -/* Helper function to convert a PXCPoint3DF32 point into a PCL point. - * Takes care of unit conversion (PXC point coordinates are in millimeters) - * and invalid points. */ -template inline void -convertPoint (const PXCPoint3DF32& src, T& tgt) -{ - static const float nan = std::numeric_limits::quiet_NaN (); - if (src.z == 0) - { - tgt.x = tgt.y = tgt.z = nan; - } - else - { - tgt.x = -src.x / 1000.0; - tgt.y = src.y / 1000.0; - tgt.z = src.z / 1000.0; - } -} - pcl::RealSenseGrabber::Mode::Mode () : fps (0), depth_width (0), depth_height (0), color_width (0), color_height (0) { @@ -112,26 +87,6 @@ pcl::RealSenseGrabber::Mode::operator== (const pcl::RealSenseGrabber::Mode& m) c this->color_height == m.color_height); } -pcl::RealSenseGrabber::RealSenseGrabber (const std::string& device_id, const Mode& mode, bool strict) -: Grabber () -, is_running_ (false) -, confidence_threshold_ (6) -, temporal_filtering_type_ (RealSense_None) -, temporal_filtering_window_size_ (1) -, mode_requested_ (mode) -, strict_ (strict) -{ - if (device_id == "") - device_ = RealSenseDeviceManager::getInstance ()->captureDevice (); - else if (device_id[0] == '#') - device_ = RealSenseDeviceManager::getInstance ()->captureDevice (boost::lexical_cast (device_id.substr (1)) - 1); - else - device_ = RealSenseDeviceManager::getInstance ()->captureDevice (device_id); - - point_cloud_signal_ = createSignal (); - point_cloud_rgba_signal_ = createSignal (); -} - pcl::RealSenseGrabber::~RealSenseGrabber () throw () { stop (); @@ -140,43 +95,6 @@ pcl::RealSenseGrabber::~RealSenseGrabber () throw () disconnect_all_slots (); } -void -pcl::RealSenseGrabber::start () -{ - if (!is_running_) - { - need_xyz_ = num_slots () > 0; - need_xyzrgba_ = num_slots () > 0; - if (need_xyz_ || need_xyzrgba_) - { - selectMode (); - PXCCapture::Device::StreamProfileSet profile; - memset (&profile, 0, sizeof (profile)); - profile.depth.frameRate.max = mode_selected_.fps; - profile.depth.frameRate.min = mode_selected_.fps; - profile.depth.imageInfo.width = mode_selected_.depth_width; - profile.depth.imageInfo.height = mode_selected_.depth_height; - profile.depth.imageInfo.format = PXCImage::PIXEL_FORMAT_DEPTH; - profile.depth.options = PXCCapture::Device::STREAM_OPTION_ANY; - if (need_xyzrgba_) - { - profile.color.frameRate.max = mode_selected_.fps; - profile.color.frameRate.min = mode_selected_.fps; - profile.color.imageInfo.width = mode_selected_.color_width; - profile.color.imageInfo.height = mode_selected_.color_height; - profile.color.imageInfo.format = PXCImage::PIXEL_FORMAT_RGB32; - profile.color.options = PXCCapture::Device::STREAM_OPTION_ANY; - } - device_->getPXCDevice ().SetStreamProfileSet (&profile); - if (!device_->getPXCDevice ().IsStreamProfileSetValid (&profile)) - THROW_IO_EXCEPTION ("Invalid stream profile for PXC device"); - frequency_.reset (); - is_running_ = true; - thread_ = boost::thread (&RealSenseGrabber::run, this); - } - } -} - void pcl::RealSenseGrabber::stop () { @@ -200,20 +118,6 @@ pcl::RealSenseGrabber::getFramesPerSecond () const return (frequency_.getFrequency ()); } -void -pcl::RealSenseGrabber::setConfidenceThreshold (unsigned int threshold) -{ - if (threshold > 15) - { - PCL_WARN ("[pcl::RealSenseGrabber::setConfidenceThreshold] Attempted to set threshold outside valid range (0-15)"); - } - else - { - confidence_threshold_ = threshold; - device_->getPXCDevice ().SetDepthConfidenceThreshold (confidence_threshold_); - } -} - void pcl::RealSenseGrabber::enableTemporalFiltering (TemporalFilteringType type, size_t window_size) { @@ -236,46 +140,6 @@ pcl::RealSenseGrabber::disableTemporalFiltering () enableTemporalFiltering (RealSense_None, 1); } -const std::string& -pcl::RealSenseGrabber::getDeviceSerialNumber () const -{ - return (device_->getSerialNumber ()); -} - -std::vector -pcl::RealSenseGrabber::getAvailableModes (bool only_depth) const -{ - std::vector modes; - PXCCapture::StreamType streams = only_depth - ? PXCCapture::STREAM_TYPE_DEPTH - : PXCCapture::STREAM_TYPE_DEPTH | PXCCapture::STREAM_TYPE_COLOR; - for (int p = 0;; p++) - { - PXCCapture::Device::StreamProfileSet profiles = {}; - if (device_->getPXCDevice ().QueryStreamProfileSet (streams, p, &profiles) == PXC_STATUS_NO_ERROR) - { - if (!only_depth && profiles.depth.frameRate.max != profiles.color.frameRate.max) - continue; // we need both streams to have the same framerate - Mode mode; - mode.fps = profiles.depth.frameRate.max; - mode.depth_width = profiles.depth.imageInfo.width; - mode.depth_height = profiles.depth.imageInfo.height; - mode.color_width = profiles.color.imageInfo.width; - mode.color_height = profiles.color.imageInfo.height; - bool duplicate = false; - for (size_t i = 0; i < modes.size (); ++i) - duplicate |= modes[i] == mode; - if (!duplicate) - modes.push_back (mode); - } - else - { - break; - } - } - return modes; -} - void pcl::RealSenseGrabber::setMode (const Mode& mode, bool strict) { @@ -290,139 +154,6 @@ pcl::RealSenseGrabber::setMode (const Mode& mode, bool strict) } } -void -pcl::RealSenseGrabber::run () -{ - const int WIDTH = mode_selected_.depth_width; - const int HEIGHT = mode_selected_.depth_height; - const int SIZE = WIDTH * HEIGHT; - - PXCProjection* projection = device_->getPXCDevice ().CreateProjection (); - PXCCapture::Sample sample; - std::vector vertices (SIZE); - createDepthBuffer (); - - while (is_running_) - { - pcl::PointCloud::Ptr xyz_cloud; - pcl::PointCloud::Ptr xyzrgba_cloud; - - pxcStatus status; - if (need_xyzrgba_) - status = device_->getPXCDevice ().ReadStreams (PXCCapture::STREAM_TYPE_DEPTH | PXCCapture::STREAM_TYPE_COLOR, &sample); - else - status = device_->getPXCDevice ().ReadStreams (PXCCapture::STREAM_TYPE_DEPTH, &sample); - - uint64_t timestamp = pcl::getTime () * 1.0e+6; - - switch (status) - { - case PXC_STATUS_NO_ERROR: - { - fps_mutex_.lock (); - frequency_.event (); - fps_mutex_.unlock (); - - /* We preform the following steps to convert received data into point clouds: - * - * 1. Push depth image to the depth buffer - * 2. Pull filtered depth image from the depth buffer - * 3. Project (filtered) depth image into 3D - * 4. Fill XYZ point cloud with computed points - * 5. Fill XYZRGBA point cloud with computed points - * 7. Project color image into 3D - * 6. Assign colors to points in XYZRGBA point cloud - * - * Steps 1-2 are skipped if temporal filtering is disabled. - * Step 4 is skipped if there are no subscribers for XYZ clouds. - * Steps 5-7 are skipped if there are no subscribers for XYZRGBA clouds. */ - - if (temporal_filtering_type_ != RealSense_None) - { - PXCImage::ImageData data; - sample.depth->AcquireAccess (PXCImage::ACCESS_READ, &data); - std::vector data_copy (SIZE); - memcpy (data_copy.data (), data.planes[0], SIZE * sizeof (unsigned short)); - sample.depth->ReleaseAccess (&data); - - depth_buffer_->push (data_copy); - - sample.depth->AcquireAccess (PXCImage::ACCESS_WRITE, &data); - unsigned short* d = reinterpret_cast (data.planes[0]); - for (size_t i = 0; i < SIZE; i++) - d[i] = (*depth_buffer_)[i]; - sample.depth->ReleaseAccess (&data); - } - - projection->QueryVertices (sample.depth, vertices.data ()); - - if (need_xyz_) - { - xyz_cloud.reset (new pcl::PointCloud (WIDTH, HEIGHT)); - xyz_cloud->header.stamp = timestamp; - xyz_cloud->is_dense = false; - for (int i = 0; i < SIZE; i++) - convertPoint (vertices[i], xyz_cloud->points[i]); - } - - if (need_xyzrgba_) - { - PXCImage::ImageData data; - PXCImage* mapped = projection->CreateColorImageMappedToDepth (sample.depth, sample.color); - mapped->AcquireAccess (PXCImage::ACCESS_READ, &data); - uint32_t* d = reinterpret_cast (data.planes[0]); - if (need_xyz_) - { - // We can fill XYZ coordinates more efficiently using pcl::copyPointCloud, - // given that they were already computed for XYZ point cloud. - xyzrgba_cloud.reset (new pcl::PointCloud); - pcl::copyPointCloud (*xyz_cloud, *xyzrgba_cloud); - for (int i = 0; i < HEIGHT; i++) - { - pcl::PointXYZRGBA* cloud_row = &xyzrgba_cloud->points[i * WIDTH]; - uint32_t* color_row = &d[i * data.pitches[0] / sizeof (uint32_t)]; - for (int j = 0; j < WIDTH; j++) - memcpy (&cloud_row[j].rgba, &color_row[j], sizeof (uint32_t)); - } - } - else - { - xyzrgba_cloud.reset (new pcl::PointCloud (WIDTH, HEIGHT)); - xyzrgba_cloud->header.stamp = timestamp; - xyzrgba_cloud->is_dense = false; - for (int i = 0; i < HEIGHT; i++) - { - PXCPoint3DF32* vertices_row = &vertices[i * WIDTH]; - pcl::PointXYZRGBA* cloud_row = &xyzrgba_cloud->points[i * WIDTH]; - uint32_t* color_row = &d[i * data.pitches[0] / sizeof (uint32_t)]; - for (int j = 0; j < WIDTH; j++) - { - convertPoint (vertices_row[j], cloud_row[j]); - memcpy (&cloud_row[j].rgba, &color_row[j], sizeof (uint32_t)); - } - } - } - mapped->ReleaseAccess (&data); - mapped->Release (); - } - - if (need_xyzrgba_) - point_cloud_rgba_signal_->operator () (xyzrgba_cloud); - if (need_xyz_) - point_cloud_signal_->operator () (xyz_cloud); - break; - } - case PXC_STATUS_DEVICE_LOST: - THROW_IO_EXCEPTION ("failed to read data stream from PXC device: device lost"); - case PXC_STATUS_ALLOC_FAILED: - THROW_IO_EXCEPTION ("failed to read data stream from PXC device: alloc failed"); - } - sample.ReleaseImages (); - } - projection->Release (); - RealSenseDevice::reset (device_); -} - float pcl::RealSenseGrabber::computeModeScore (const Mode& mode) { @@ -461,7 +192,7 @@ pcl::RealSenseGrabber::selectMode () } } if (strict_ && best_score > 0) - THROW_IO_EXCEPTION ("PXC device does not support requested mode"); + THROW_IO_EXCEPTION ("RealSense device does not support requested mode"); } void @@ -487,4 +218,3 @@ pcl::RealSenseGrabber::createDepthBuffer () } } } - diff --git a/visualization/CMakeLists.txt b/visualization/CMakeLists.txt index 940fc8ef2d9..3a0e247e501 100644 --- a/visualization/CMakeLists.txt +++ b/visualization/CMakeLists.txt @@ -14,7 +14,7 @@ else(NOT VTK_FOUND) endif(NOT VTK_FOUND) PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ${DEFAULT} "${REASON}") -PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS vtk OPT_DEPS openni openni2 ensenso davidSDK dssdk rssdk) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS vtk OPT_DEPS openni openni2 ensenso davidSDK dssdk rssdk librealsense) if (ANDROID) set (build FALSE) @@ -181,6 +181,9 @@ if(build) if(WITH_RSSDK) list(APPEND EXT_DEPS rssdk) endif() + if(WITH_LIBREALSENSE) + list(APPEND EXT_DEPS librealsense) + endif() PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "") diff --git a/visualization/tools/CMakeLists.txt b/visualization/tools/CMakeLists.txt index 2b515f58d49..272b8e0cd93 100644 --- a/visualization/tools/CMakeLists.txt +++ b/visualization/tools/CMakeLists.txt @@ -60,5 +60,7 @@ endif() if(WITH_RSSDK) PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_real_sense_viewer ${SUBSYS_NAME} real_sense_viewer.cpp) target_link_libraries(pcl_real_sense_viewer pcl_common pcl_io pcl_visualization) +elseif(WITH_LIBREALSENSE) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_real_sense_viewer ${SUBSYS_NAME} real_sense_viewer.cpp) + target_link_libraries(pcl_real_sense_viewer pcl_common pcl_io pcl_visualization) endif() - From 2911593fd89979d77fea0a5dea861f079f056607 Mon Sep 17 00:00:00 2001 From: lebronzhang Date: Thu, 14 Jul 2016 06:29:36 +0800 Subject: [PATCH 2/3] Add static link support to librealsense --- cmake/Modules/Findlibrealsense.cmake | 35 ++++++++++++++++++++++------ 1 file changed, 28 insertions(+), 7 deletions(-) diff --git a/cmake/Modules/Findlibrealsense.cmake b/cmake/Modules/Findlibrealsense.cmake index 82bcdc0387c..6517d643aec 100644 --- a/cmake/Modules/Findlibrealsense.cmake +++ b/cmake/Modules/Findlibrealsense.cmake @@ -13,19 +13,40 @@ if (LIBREALSENSE_LIBRARIES AND LIBREALSENSE_INCLUDE_DIRS) # in cache already set(LIBREALSENSE_FOUND TRUE) else (LIBREALSENSE_LIBRARIES AND LIBREALSENSE_INCLUDE_DIRS) + find_path(LIBREALSENSE_DIR include/librealsense + PATHS $ENV{LIBREALSENSE_DIR} NO_DEFAULT_PATH) + if (WIN32) + set(LIBREALSENSE_RELEASE_NAME realsense.lib) + set(LIBREALSENSE_DEBUG_NAME realsense-d.lib) + set(LIBREALSENSE_SUFFIX Win32) + if (CMAKE_SIZEOF_VOID_P EQUAL 8) + set(LIBREALSENSE_SUFFIX x64) + endif () + else (WIN32) + set(LIBREALSENSE_RELEASE_NAME librealsense.so) + set(LIBREALSENSE_DEBUG_NAME librealsense.a) + endif (WIN32) + find_path(LIBREALSENSE_INCLUDE_DIR NAMES librealsense - PATHS /usr/local/include /usr/include /opt/local/include /sw/include) + PATHS /usr/local/include /usr/include /opt/local/include /sw/include ${LIBREALSENSE_DIR}/include NO_DEFAULT_PATH) find_library(LIBREALSENSE_LIBRARY - NAMES librealsense.so - PATHS /usr/local/lib /usr/lib /opt/local/lib /sw/lib) + NAMES ${LIBREALSENSE_RELEASE_NAME} + PATHS ${LIBREALSENSE_DIR}/bin/ /usr/local/lib /usr/lib /opt/local/lib /sw/lib ${LIBREALSENSE_DIR}/lib/ NO_DEFAULT_PATH + PATH_SUFFIXES ${LIBREALSENSE_SUFFIX}) +find_library(LIBREALSENSE_LIBRARY_DEBUG + NAMES ${LIBREALSENSE_DEBUG_NAME} + PATHS ${LIBREALSENSE_DIR}/bin/ ${LIBREALSENSE_DIR}/bin/debug/ NO_DEFAULT_PATH + PATH_SUFFIXES ${LIBREALSENSE_SUFFIX}) +if (NOT LIBREALSENSE_LIBRARY_DEBUG) + set(LIBREALSENSE_LIBRARY_DEBUG ${LIBREALSENSE_LIBRARY}) +endif () +set(LIBREALSENSE_LIBRARIES optimized ${LIBREALSENSE_LIBRARY} debug ${LIBREALSENSE_LIBRARY_DEBUG}) set(LIBREALSENSE_INCLUDE_DIRS ${LIBREALSENSE_INCLUDE_DIR}) -set(LIBREALSENSE_LIBRARIES ${LIBREALSENSE_LIBRARY}) include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(LIBREALSENSE LIBREALSENSE_LIBRARY LIBREALSENSE_INCLUDE_DIR) +find_package_handle_standard_args(LIBREALSENSE LIBREALSENSE_LIBRARIES LIBREALSENSE_INCLUDE_DIRS) -mark_as_advanced(LIBREALSENSE_INCLUDE_DIRS LIBREALSENSE_LIBRARIES) -endif (LIBREALSENSE_LIBRARIES AND LIBREALSENSE_INCLUDE_DIRS) +endif (LIBREALSENSE_LIBRARIES AND LIBREALSENSE_INCLUDE_DIRS) \ No newline at end of file From dd638418284601041071ff69afd907bcff669636 Mon Sep 17 00:00:00 2001 From: lebron zhang Date: Sat, 16 Jul 2016 16:39:53 +0800 Subject: [PATCH 3/3] Add new search path to librealsense debug library --- cmake/Modules/Findlibrealsense.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/Modules/Findlibrealsense.cmake b/cmake/Modules/Findlibrealsense.cmake index 6517d643aec..a0b9e278186 100644 --- a/cmake/Modules/Findlibrealsense.cmake +++ b/cmake/Modules/Findlibrealsense.cmake @@ -37,7 +37,7 @@ find_library(LIBREALSENSE_LIBRARY PATH_SUFFIXES ${LIBREALSENSE_SUFFIX}) find_library(LIBREALSENSE_LIBRARY_DEBUG NAMES ${LIBREALSENSE_DEBUG_NAME} - PATHS ${LIBREALSENSE_DIR}/bin/ ${LIBREALSENSE_DIR}/bin/debug/ NO_DEFAULT_PATH + PATHS ${LIBREALSENSE_DIR}/bin/ ${LIBREALSENSE_DIR}/bin/debug/ ${LIBREALSENSE_DIR}/lib/ NO_DEFAULT_PATH PATH_SUFFIXES ${LIBREALSENSE_SUFFIX}) if (NOT LIBREALSENSE_LIBRARY_DEBUG) set(LIBREALSENSE_LIBRARY_DEBUG ${LIBREALSENSE_LIBRARY})