From 551c9e91c23d3bee4aefed1a35e5a159bb4f10a2 Mon Sep 17 00:00:00 2001 From: Ky Waegel Date: Mon, 3 Mar 2014 05:19:49 -0500 Subject: [PATCH 01/12] Added libboost-chrono1.53-dev to Travis CI script. --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index cc63692d5c3..394ef4947e2 100644 --- a/.travis.yml +++ b/.travis.yml @@ -41,6 +41,6 @@ before_install: - sudo add-apt-repository ppa:libreoffice/ppa -y - sudo apt-get update -d install: - - sudo apt-get install cmake libvtk5-qt4-dev libflann-dev libeigen3-dev libopenni-dev libqhull-dev libboost-filesystem1.53-dev libboost-iostreams1.53-dev libboost-thread1.53-dev + - sudo apt-get install cmake libvtk5-qt4-dev libflann-dev libeigen3-dev libopenni-dev libqhull-dev libboost-filesystem1.53-dev libboost-iostreams1.53-dev libboost-thread1.53-dev libboost-chrono1.53-dev script: - bash .travis.sh From 0733d4754abb4caa8ce517f43fff091ce888af20 Mon Sep 17 00:00:00 2001 From: Ky Waegel Date: Sun, 2 Mar 2014 23:15:32 -0500 Subject: [PATCH 02/12] Added boost::chrono to dependency list * Chrono was added in boost 1.47.0, so changed minimum version to match. --- cmake/pcl_find_boost.cmake | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/cmake/pcl_find_boost.cmake b/cmake/pcl_find_boost.cmake index 5c1a70640f6..c93eaecc2b3 100644 --- a/cmake/pcl_find_boost.cmake +++ b/cmake/pcl_find_boost.cmake @@ -23,7 +23,7 @@ endif(${CMAKE_VERSION} VERSION_LESS 2.8.5) set(Boost_NO_BOOST_CMAKE ON) # Optional boost modules -find_package(Boost 1.40.0 QUIET COMPONENTS serialization mpi) +find_package(Boost 1.47.0 QUIET COMPONENTS serialization mpi) if(Boost_MPI_FOUND) set(BOOST_MPI_FOUND TRUE) endif(Boost_MPI_FOUND) @@ -32,15 +32,9 @@ if(Boost_SERIALIZATION_FOUND) endif(Boost_SERIALIZATION_FOUND) # Required boost modules -set(BOOST_REQUIRED_MODULES system filesystem thread date_time iostreams) -# Starting with Boost 1.50, boost_thread depends on chrono. As this is not -# taken care of automatically on Windows, we add an explicit dependency as a -# workaround. -if(WIN32 AND Boost_VERSION VERSION_GREATER "104900") - set(BOOST_REQUIRED_MODULES ${BOOST_REQUIRED_MODULES} chrono) -endif(WIN32 AND Boost_VERSION VERSION_GREATER "104900") +set(BOOST_REQUIRED_MODULES system filesystem thread date_time iostreams chrono) -find_package(Boost 1.40.0 REQUIRED COMPONENTS ${BOOST_REQUIRED_MODULES}) +find_package(Boost 1.47.0 REQUIRED COMPONENTS ${BOOST_REQUIRED_MODULES}) if(Boost_FOUND) set(BOOST_FOUND TRUE) From e8e641ee82567699729be95a4afe533fe6be4f5f Mon Sep 17 00:00:00 2001 From: Ky Waegel Date: Wed, 14 Aug 2013 23:28:22 -0400 Subject: [PATCH 03/12] Added FindOpenNI2.cmake search module. *also added a find_openni2 macro to PCLConfig.cmake.in to allow external users of PCL to find the OpenNI 2 libraries. This is a duplicate of the code in FindOpenNI2.cmake. --- CMakeLists.txt | 10 +++++ PCLConfig.cmake.in | 41 ++++++++++++++++- cmake/Modules/FindOpenNI2.cmake | 78 +++++++++++++++++++++++++++++++++ pcl_config.h.in | 6 +++ 4 files changed, 134 insertions(+), 1 deletion(-) create mode 100644 cmake/Modules/FindOpenNI2.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index ebbe5540a74..ef3d926bbc5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -265,6 +265,16 @@ if(WITH_OPENNI) endif(OPENNI_FOUND) endif(WITH_OPENNI) +# OpenNI 2 +option(WITH_OPENNI2 "OpenNI 2 driver support" TRUE) +if(WITH_OPENNI2) + find_package(OpenNI2) + if (OPENNI2_FOUND) + set(HAVE_OPENNI2 ON) + include_directories(SYSTEM ${OPENNI2_INCLUDE_DIRS}) + endif(OPENNI2_FOUND) +endif(WITH_OPENNI2) + # Fotonic (FZ_API) option(WITH_FZAPI "Build Fotonic Camera support" TRUE) if(WITH_FZAPI) diff --git a/PCLConfig.cmake.in b/PCLConfig.cmake.in index 92e3ed67fc8..d0cf8497c87 100644 --- a/PCLConfig.cmake.in +++ b/PCLConfig.cmake.in @@ -211,6 +211,43 @@ macro(find_openni) endif(OPENNI_FOUND) endmacro(find_openni) +#remove this as soon as openni2-dev is shipped with FindOpenni2.cmake +macro(find_openni2) + if(NOT OPENNI2_ROOT AND ("ON" STREQUAL "ON")) + get_filename_component(OPENNI2_LIBRARY_HINT "OPENNI_LIBRARY-NOTFOUND" PATH) + endif(NOT OPENNI2_ROOT AND ("ON" STREQUAL "ON")) + + set(OPENNI2_SUFFIX) + if(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) + set(OPENNI2_SUFFIX 64) + endif(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) + + if(PKG_CONFIG_FOUND) + pkg_check_modules(PC_OPENNI2 openni2-dev) + endif(PKG_CONFIG_FOUND) + + find_path(OPENNI2_INCLUDE_DIRS OpenNI.h + HINTS /usr/include/openni2 /usr/include/ni2 + PATHS "$ENV{OPENNI2_INCLUDE${OPENNI2_SUFFIX}}" + PATH_SUFFIXES openni openni2 include Include) + + find_library(OPENNI2_LIBRARY + NAMES OpenNI2 # No suffix needed on Win64 + HINTS /usr/lib + PATHS "$ENV{OPENNI2_LIB${OPENNI2_SUFFIX}}" + PATH_SUFFIXES lib Lib Lib64) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(OpenNI2 DEFAULT_MSG OPENNI2_LIBRARY OPENNI2_INCLUDE_DIRS) + + if(OPENNI2_FOUND) + get_filename_component(OPENNI_LIBRARY_PATH ${OPENNI2_LIBRARY} PATH) + set(OPENNI2_LIBRARY_DIRS ${OPENNI2_LIBRARY_PATH}) + set(OPENNI2_LIBRARIES "${OPENNI2_LIBRARY}") + set(OPENNI2_REDIST_DIR $ENV{OPENNI2_REDIST${OPENNI2_SUFFIX}}) + endif(OPENNI2_FOUND) +endmacro(find_openni2) + #remove this as soon as flann is shipped with FindFlann.cmake macro(find_flann) if(PCL_ALL_IN_ONE_INSTALLER) @@ -404,6 +441,8 @@ macro(find_external_library _component _lib _is_optional) find_qhull() elseif("${_lib}" STREQUAL "openni") find_openni() + elseif("${_lib}" STREQUAL "openni2") + find_openni2() elseif("${_lib}" STREQUAL "vtk") find_VTK() elseif("${_lib}" STREQUAL "libusb-1.0") @@ -680,7 +719,7 @@ endif(NOT "${PCL_DEFINITIONS}" STREQUAL "") pcl_remove_duplicate_libraries(PCL_LIBRARIES PCL_DEDUP_LIBRARIES) set(PCL_LIBRARIES ${PCL_DEDUP_LIBRARIES}) # Add 3rd party libraries, as user code might include our .HPP implementations -list(APPEND PCL_LIBRARIES ${BOOST_LIBRARIES} ${QHULL_LIBRARIES} ${OPENNI_LIBRARIES} ${FLANN_LIBRARIES} ${VTK_LIBRARIES}) +list(APPEND PCL_LIBRARIES ${BOOST_LIBRARIES} ${QHULL_LIBRARIES} ${OPENNI_LIBRARIES} ${OPENNI2_LIBRARIES} ${FLANN_LIBRARIES} ${VTK_LIBRARIES}) find_package_handle_standard_args(PCL DEFAULT_MSG PCL_LIBRARIES PCL_INCLUDE_DIRS) mark_as_advanced(PCL_LIBRARIES PCL_INCLUDE_DIRS PCL_LIBRARY_DIRS) diff --git a/cmake/Modules/FindOpenNI2.cmake b/cmake/Modules/FindOpenNI2.cmake new file mode 100644 index 00000000000..a33fa343aca --- /dev/null +++ b/cmake/Modules/FindOpenNI2.cmake @@ -0,0 +1,78 @@ +############################################################################### +# Find OpenNI 2 +# +# This sets the following variables: +# OPENNI2_FOUND - True if OPENNI 2 was found. +# OPENNI2_INCLUDE_DIRS - Directories containing the OPENNI 2 include files. +# OPENNI2_LIBRARIES - Libraries needed to use OPENNI 2. +# OPENNI2_DEFINITIONS - Compiler flags for OPENNI 2. +# +# For libusb-1.0, add USB_10_ROOT if not found + +find_package(PkgConfig QUIET) + +# Find LibUSB +if(NOT WIN32) + pkg_check_modules(PC_USB_10 libusb-1.0) + find_path(USB_10_INCLUDE_DIR libusb-1.0/libusb.h + HINTS ${PC_USB_10_INCLUDEDIR} ${PC_USB_10_INCLUDE_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}" + PATH_SUFFIXES libusb-1.0) + + find_library(USB_10_LIBRARY + NAMES usb-1.0 + HINTS ${PC_USB_10_LIBDIR} ${PC_USB_10_LIBRARY_DIRS} "${USB_10_ROOT}" "$ENV{USB_10_ROOT}" + PATH_SUFFIXES lib) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(USB_10 DEFAULT_MSG USB_10_LIBRARY USB_10_INCLUDE_DIR) + + if(NOT USB_10_FOUND) + message(STATUS "OpenNI 2 disabled because libusb-1.0 not found.") + return() + else() + include_directories(SYSTEM ${USB_10_INCLUDE_DIR}) + endif() +endif(NOT WIN32) + +if(${CMAKE_VERSION} VERSION_LESS 2.8.2) + pkg_check_modules(PC_OPENNI2 openni2-dev) +else() + pkg_check_modules(PC_OPENNI2 QUIET openni2-dev) +endif() + +set(OPENNI2_DEFINITIONS ${PC_OPENNI_CFLAGS_OTHER}) + +set(OPENNI2_SUFFIX) +if(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) + set(OPENNI2_SUFFIX 64) +endif(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) + +find_path(OPENNI2_INCLUDE_DIRS OpenNI.h + PATHS "$ENV{OPENNI2_INCLUDE${OPENNI2_SUFFIX}}" # Linux/Windows default path, Win64 needs '64' suffix + ) + +find_library(OPENNI2_LIBRARY + NAMES OpenNI2 # No suffix needed on Win64 + libOpenNI2 # Linux + PATHS "$ENV{OPENNI2_LIB${OPENNI2_SUFFIX}}" # Windows default path, Win64 needs '64' suffix + "$ENV{OPENNI2_REDIST}" # Linux install does not use a separate 'lib' directory + ) + +if(CMAKE_SYSTEM_NAME STREQUAL "Darwin") + set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY} ${LIBUSB_1_LIBRARIES}) +else() + set(OPENNI2_LIBRARIES ${OPENNI2_LIBRARY}) +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(OpenNI2 DEFAULT_MSG OPENNI2_LIBRARY OPENNI2_INCLUDE_DIRS) + +mark_as_advanced(OPENNI2_LIBRARY OPENNI2_INCLUDE_DIRS) + +if(OPENNI2_FOUND) + # Add the include directories + set(OPENNI2_INCLUDE_DIRS ${OPENNI2_INCLUDE_DIR}) + set(OPENNI2_REDIST_DIR $ENV{OPENNI2_REDIST${OPENNI2_SUFFIX}}) + message(STATUS "OpenNI 2 found (include: ${OPENNI2_INCLUDE_DIRS}, lib: ${OPENNI2_LIBRARY}, redist: ${OPENNI2_REDIST_DIR})") +endif(OPENNI2_FOUND) + diff --git a/pcl_config.h.in b/pcl_config.h.in index 5a22a39b7a6..f8b4cb37dbc 100644 --- a/pcl_config.h.in +++ b/pcl_config.h.in @@ -16,6 +16,8 @@ #cmakedefine HAVE_OPENNI 1 +#cmakedefine HAVE_OPENNI2 1 + #cmakedefine HAVE_QHULL 1 #cmakedefine HAVE_QHULL_2011 1 @@ -44,6 +46,10 @@ #undef HAVE_OPENNI #endif +#ifdef DISABLE_OPENNI2 +#undef HAVE_OPENNI2 +#endif + #ifdef DISABLE_QHULL #undef HAVE_QHULL #endif From f780d48f0ee75c422e9a489b4b627d95abf13616 Mon Sep 17 00:00:00 2001 From: Ky Waegel Date: Sun, 16 Feb 2014 22:06:08 -0500 Subject: [PATCH 04/12] Created abstract Image interface for OpenNI grabbers. --- io/CMakeLists.txt | 21 ++ io/include/pcl/io/image.h | 215 +++++++++++++++ io/include/pcl/io/image_depth.h | 191 +++++++++++++ io/include/pcl/io/image_ir.h | 114 ++++++++ io/include/pcl/io/image_metadata_wrapper.h | 82 ++++++ io/include/pcl/io/image_rgb24.h | 91 +++++++ io/include/pcl/io/image_yuv422.h | 79 ++++++ io/include/pcl/io/io_exception.h | 107 ++++++++ io/src/image_depth.cpp | 302 +++++++++++++++++++++ io/src/image_ir.cpp | 160 +++++++++++ io/src/image_rgb24.cpp | 160 +++++++++++ io/src/image_yuv422.cpp | 160 +++++++++++ io/src/io_exception.cpp | 83 ++++++ 13 files changed, 1765 insertions(+) create mode 100644 io/include/pcl/io/image.h create mode 100644 io/include/pcl/io/image_depth.h create mode 100644 io/include/pcl/io/image_ir.h create mode 100644 io/include/pcl/io/image_metadata_wrapper.h create mode 100644 io/include/pcl/io/image_rgb24.h create mode 100644 io/include/pcl/io/image_yuv422.h create mode 100644 io/include/pcl/io/io_exception.h create mode 100644 io/src/image_depth.cpp create mode 100644 io/src/image_ir.cpp create mode 100644 io/src/image_rgb24.cpp create mode 100644 io/src/image_yuv422.cpp create mode 100644 io/src/io_exception.cpp diff --git a/io/CMakeLists.txt b/io/CMakeLists.txt index f78857adb6b..d42c7e90f3a 100644 --- a/io/CMakeLists.txt +++ b/io/CMakeLists.txt @@ -13,6 +13,24 @@ endif(WIN32) PCL_ADD_DOC("${SUBSYS_NAME}") if(build) + set(IMAGE_INCLUDES + include/pcl/io/image_metadata_wrapper.h + include/pcl/io/image.h + include/pcl/io/image_rgb24.h + include/pcl/io/image_yuv422.h + include/pcl/io/image_ir.h + include/pcl/io/image_depth.h + include/pcl/io/io_exception.h + ) + + set(IMAGE_SOURCES + src/image_rgb24.cpp + src/image_yuv422.cpp + src/image_ir.cpp + src/image_depth.cpp + src/io_exception.cpp + ) + OPTION(BUILD_OPENNI "Build the OpenNI Grabber." ON) MARK_AS_ADVANCED(BUILD_OPENNI) if(NOT BUILD_OPENNI) @@ -59,6 +77,9 @@ if(build) ) endif(OPENNI_FOUND) + source_group("Image Headers" FILES ${IMAGE_INCLUDES}) + source_group("Image Sources" FILES ${IMAGE_SOURCES}) + if(FZAPI_FOUND) set(FZAPI_GRABBER_INCLUDES include/pcl/io/fotonic_grabber.h diff --git a/io/include/pcl/io/image.h b/io/include/pcl/io/image.h new file mode 100644 index 00000000000..35ea69ba1fa --- /dev/null +++ b/io/include/pcl/io/image.h @@ -0,0 +1,215 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, 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 +#ifndef PCL_IO_IMAGE_H_ +#define PCL_IO_IMAGE_H_ + +#include +#include +#include + +#include + +namespace pcl +{ + namespace io + { + + /** + * @brief Image interface class providing an interface to fill a RGB or Grayscale image buffer. + * @param[in] image_metadata + * @ingroup io + */ + class PCL_EXPORTS Image + { + public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + typedef boost::chrono::high_resolution_clock Clock; + typedef boost::chrono::high_resolution_clock::time_point Timestamp; + + typedef enum + { + BAYER_GRBG, + YUV422, + RGB + } Encoding; + + Image (FrameWrapper::Ptr image_metadata) + : wrapper_ (image_metadata) + , timestamp_ (Clock::now ()) + {} + + Image (FrameWrapper::Ptr image_metadata, Timestamp time) + : wrapper_ (image_metadata) + , timestamp_ (time) + {} + + /** + * @brief virtual Destructor that never throws an exception. + */ + inline virtual ~Image () + {} + + /** + * @param[in] input_width width of input image + * @param[in] input_height height of input image + * @param[in] output_width width of desired output image + * @param[in] output_height height of desired output image + * @return wheter the resizing is supported or not. + */ + virtual bool + isResizingSupported (unsigned input_width, unsigned input_height, + unsigned output_width, unsigned output_height) const = 0; + + /** + * @brief fills a user given buffer with the RGB values, with an optional nearest-neighbor down sampling and an optional subregion + * @param[in] width desired width of output image. + * @param[in] height desired height of output image. + * @param[in,out] rgb_buffer the output RGB buffer. + * @param[in] rgb_line_step optional line step in bytes to allow the output in a rectangular subregion of the output buffer. + */ + virtual void + fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const = 0; + + /** + * @brief returns the encoding of the native data. + * @return encoding + */ + virtual Encoding + getEncoding () const = 0; + + /** + * @brief fills a user given buffer with the raw values. + * @param[in,out] rgb_buffer + */ + virtual void + fillRaw (unsigned char* rgb_buffer) const + { + memcpy (rgb_buffer, wrapper_->getData (), wrapper_->getDataSize ()); + } + + /** + * @brief fills a user given buffer with the gray values, with an optional nearest-neighbor down sampling and an optional subregion + * @param[in] width desired width of output image. + * @param[in] height desired height of output image. + * @param[in,out] gray_buffer the output gray buffer. + * @param[in] gray_line_step optional line step in bytes to allow the output in a rectangular subregion of the output buffer. + */ + virtual void + fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, + unsigned gray_line_step = 0) const = 0; + + /** + * @return width of the image + */ + unsigned + getWidth () const + { + return (wrapper_->getWidth ()); + } + + /** + * @return height of the image + */ + unsigned + getHeight () const + { + return (wrapper_->getHeight ()); + } + + /** + * @return frame id of the image. + * @note frame ids are ascending, but not necessarily synchronized with other streams + */ + unsigned + getFrameID () const + { + return (wrapper_->getFrameID ()); + } + + /** + * @return the timestamp of the image + * @note the time value is not synchronized with the system time + */ + uint64_t + getTimestamp () const + { + return (wrapper_->getTimestamp ()); + } + + + /** + * @return the timestamp of the image + * @note the time value *is* synchronized with the system time. + */ + Timestamp + getSystemTimestamp () const + { + return (timestamp_); + } + + // Get a const pointer to the raw depth buffer + const void* + getData () + { + return (wrapper_->getData ()); + } + + // Data buffer size in bytes + int + getDataSize () const + { + return (wrapper_->getDataSize ()); + } + + // Size of each row, including any padding + inline unsigned + getStep() const + { + return (getDataSize() / getHeight()); + } + + protected: + FrameWrapper::Ptr wrapper_; + Timestamp timestamp_; + }; + + } // namespace +} + +#endif //PCL_IO_IMAGE_H_ diff --git a/io/include/pcl/io/image_depth.h b/io/include/pcl/io/image_depth.h new file mode 100644 index 00000000000..3e00adbe5f1 --- /dev/null +++ b/io/include/pcl/io/image_depth.h @@ -0,0 +1,191 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011 Willow Garage, Inc. + * Copyright (c) 2012-, 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. + * + */ +#pragma once +#include + +#ifndef PCL_IO_IMAGE_DEPTH_H_ +#define PCL_IO_IMAGE_DEPTH_H_ + +#include +#include +#include + +#include + +namespace pcl +{ + namespace io + { + /** \brief This class provides methods to fill a depth or disparity image. + */ + class PCL_EXPORTS DepthImage + { + public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + typedef boost::chrono::high_resolution_clock Clock; + typedef boost::chrono::high_resolution_clock::time_point Timestamp; + + /** \brief Constructor + * \param[in] depth_meta_data the actual data from the OpenNI library + * \param[in] baseline the baseline of the "stereo" camera, i.e. the distance between the projector and the IR camera for + * Primesense like cameras. e.g. 7.5cm for PSDK5 and PSDK6 reference design. + * \param[in] focal_length focal length of the "stereo" frame. + * \param[in] shadow_value defines which values in the depth data are indicating shadow (resulting from the parallax between projector and IR camera) + * \param[in] no_sample_value defines which values in the depth data are indicating that no depth (disparity) could be determined . + * \attention The focal length may change, depending whether the depth stream is registered/mapped to the RGB stream or not. + */ + DepthImage (FrameWrapper::Ptr depth_metadata, float baseline, float focal_length, uint64_t shadow_value, uint64_t no_sample_value); + DepthImage (FrameWrapper::Ptr depth_metadata, float baseline, float focal_length, uint64_t shadow_value, uint64_t no_sample_value, Timestamp time); + + /** \brief Destructor. Never throws an exception. */ + ~DepthImage (); + + /** \brief method to access the internal data structure from OpenNI. If the data is accessed just read-only, then this method is faster than a fillXXX method + * \return the actual depth data of type openni::VideoFrameRef. + */ + const FrameWrapper::Ptr + getMetaData () const; + + /** \brief fills a user given block of memory with the disparity values with additional nearest-neighbor down-scaling. + * \param[in] width the width of the desired disparity image. + * \param[in] height the height of the desired disparity image. + * \param[in,out] disparity_buffer the float pointer to the actual memory buffer to be filled with the disparity values. + * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the + * width in bytes (not floats) of the original width of the depth buffer. + */ + void + fillDisparityImage (unsigned width, unsigned height, float* disparity_buffer, unsigned line_step = 0) const; + + /** \brief fills a user given block of memory with the disparity values with additional nearest-neighbor down-scaling. + * \param[in] width width the width of the desired depth image. + * \param[in] height height the height of the desired depth image. + * \param[in,out] depth_buffer the float pointer to the actual memory buffer to be filled with the depth values. + * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the + * width in bytes (not floats) of the original width of the depth buffer. + */ + void + fillDepthImage (unsigned width, unsigned height, float* depth_buffer, unsigned line_step = 0) const; + + /** \brief fills a user given block of memory with the raw values with additional nearest-neighbor down-scaling. + * \param[in] width width the width of the desired raw image. + * \param[in] height height the height of the desired raw image. + * \param[in,out] depth_buffer the unsigned short pointer to the actual memory buffer to be filled with the raw values. + * \param[in] line_step if only a rectangular sub region of the buffer needs to be filled, then line_step is the + * width in bytes (not floats) of the original width of the depth buffer. + */ + void + fillDepthImageRaw (unsigned width, unsigned height, unsigned short* depth_buffer, unsigned line_step = 0) const; + + /** \brief method to access the baseline of the "stereo" frame that was used to retrieve the depth image. + * \return baseline in meters + */ + float + getBaseline () const; + + /** \brief method to access the focal length of the "stereo" frame that was used to retrieve the depth image. + * \return focal length in pixels + */ + float + getFocalLength () const; + + /** \brief method to access the shadow value, that indicates pixels lying in shadow in the depth image. + * \return shadow value + */ + uint64_t + getShadowValue () const; + + /** \brief method to access the no-sample value, that indicates pixels where no disparity could be determined for the depth image. + * \return no-sample value + */ + uint64_t + getNoSampleValue () const; + + /** \return the width of the depth image */ + unsigned + getWidth () const; + + /** \return the height of the depth image */ + unsigned + getHeight () const; + + /** \return an ascending id for the depth frame + * \attention not necessarily synchronized with other streams + */ + unsigned + getFrameID () const; + + /** \return a ascending timestamp for the depth frame + * \attention its not the system time, thus can not be used directly to synchronize different sensors. + * But definitely synchronized with other streams + */ + uint64_t + getTimestamp () const; + + Timestamp + getSystemTimestamp () const; + + // Get a const pointer to the raw depth buffer + const unsigned short* + getData (); + + // Data buffer size in bytes + int + getDataSize () const; + + // Size of each row, including any padding + inline unsigned + getStep() const + { + return (getDataSize() / getHeight()); + } + + protected: + pcl::io::FrameWrapper::Ptr wrapper_; + + float baseline_; + float focal_length_; + uint64_t shadow_value_; + uint64_t no_sample_value_; + Timestamp timestamp_; + }; + +}} // namespace + +#endif // PCL_IO_IMAGE_DEPTH_H_ diff --git a/io/include/pcl/io/image_ir.h b/io/include/pcl/io/image_ir.h new file mode 100644 index 00000000000..a3e36a4d4fb --- /dev/null +++ b/io/include/pcl/io/image_ir.h @@ -0,0 +1,114 @@ +/* +* Software License Agreement (BSD License) +* +* Copyright (c) 2011 Willow Garage, 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. +* +*/ +#ifndef PCL_IO_IMAGE_IR_H_ +#define PCL_IO_IMAGE_IR_H_ + +#include +#include + +#include + +namespace pcl +{ + namespace io + { + + /** + * @brief Class containing just a reference to IR meta data. + */ + class PCL_EXPORTS IRImage + { + public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + typedef boost::chrono::high_resolution_clock Clock; + typedef boost::chrono::high_resolution_clock::time_point Timestamp; + + IRImage (FrameWrapper::Ptr ir_metadata); + IRImage (FrameWrapper::Ptr ir_metadata, Timestamp time); + + ~IRImage () throw () + {} + + void + fillRaw (unsigned width, unsigned height, unsigned short* ir_buffer, unsigned line_step = 0) const; + + unsigned + getWidth () const; + + unsigned + getHeight () const; + + unsigned + getFrameID () const; + + uint64_t + getTimestamp () const; + + Timestamp + getSystemTimestamp () const; + + // Get a const pointer to the raw depth buffer. If the data is accessed just read-only, then this method is faster than a fillXXX method + const unsigned short* + getData (); + + // Data buffer size in bytes + int + getDataSize () const; + + // Size of each row, including any padding + inline unsigned + getStep() const + { + return (getDataSize() / getHeight()); + } + + /** \brief method to access the internal data structure wrapper, which needs to be cast to an + * approperate subclass before the getMetadata(..) function is available to access the native data type. + */ + const FrameWrapper::Ptr + getMetaData () const; + + protected: + FrameWrapper::Ptr wrapper_; + Timestamp timestamp_; + }; + + } // namespace +} + +#endif // PCL_IO_IMAGE_IR_H_ diff --git a/io/include/pcl/io/image_metadata_wrapper.h b/io/include/pcl/io/image_metadata_wrapper.h new file mode 100644 index 00000000000..9446d625a8e --- /dev/null +++ b/io/include/pcl/io/image_metadata_wrapper.h @@ -0,0 +1,82 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, respective authors. + * + * 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. + * + */ + +#pragma once +#ifndef PCL_IO_IMAGE_METADATA_WRAPPER_H_ +#define PCL_IO_IMAGE_METADATA_WRAPPER_H_ + +#include + +namespace pcl +{ + namespace io + { + + /** + * Pure abstract interface to wrap native frame data types. + */ + class FrameWrapper + { + public: + typedef boost::shared_ptr Ptr; + + virtual const void* + getData () const = 0; + + virtual unsigned + getDataSize () const = 0; + + virtual unsigned + getWidth () const = 0; + + virtual unsigned + getHeight () const = 0; + + virtual unsigned + getFrameID () const = 0; + + // Microseconds from some arbitrary start point + virtual uint64_t + getTimestamp () const = 0; + }; + + } // namespace +} + +#endif // PCL_IO_IMAGE_METADATA_WRAPPER_H_ diff --git a/io/include/pcl/io/image_rgb24.h b/io/include/pcl/io/image_rgb24.h new file mode 100644 index 00000000000..41c155dfb57 --- /dev/null +++ b/io/include/pcl/io/image_rgb24.h @@ -0,0 +1,91 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, 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 + +#ifndef PCL_IO_IMAGE_RGB_H_ +#define PCL_IO_IMAGE_RGB_H_ + +#include +#include + +#include + +namespace pcl +{ + namespace io + { + /** + * @brief This class provides methods to fill a RGB or Grayscale image buffer from underlying RGB24 image. + * @ingroup io + */ + class PCL_EXPORTS ImageRGB24 : public pcl::io::Image + { + public: + + ImageRGB24 (FrameWrapper::Ptr image_metadata); + ImageRGB24 (FrameWrapper::Ptr image_metadata, Timestamp timestamp); + virtual ~ImageRGB24 () throw (); + + inline virtual Encoding + getEncoding () const + { + return (RGB); + } + + virtual void + fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const; + + virtual void + fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step = 0) const; + + virtual bool + isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const; + + private: + + // Struct used for type conversion + typedef struct + { + uint8_t r; + uint8_t g; + uint8_t b; + } RGB888Pixel; + }; + + } // namespace +} + +#endif // PCL_IO_IMAGE_RGB_H_ diff --git a/io/include/pcl/io/image_yuv422.h b/io/include/pcl/io/image_yuv422.h new file mode 100644 index 00000000000..61ecdd74eda --- /dev/null +++ b/io/include/pcl/io/image_yuv422.h @@ -0,0 +1,79 @@ +/* +* Software License Agreement (BSD License) +* +* Copyright (c) 2011 Willow Garage, 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 + +#ifndef PCL_IO_IMAGE_YUV422_H_ +#define PCL_IO_IMAGE_YUV422_H_ +#include + +#include + +namespace pcl +{ + namespace io + { + /** + * @brief Concrete implementation of the interface Image for a YUV 422 image used by Primesense devices. + * @ingroup io + */ + class PCL_EXPORTS ImageYUV422 : public pcl::io::Image + { + public: + ImageYUV422 (FrameWrapper::Ptr image_metadata); + ImageYUV422 (FrameWrapper::Ptr image_metadata, Timestamp timestamp); + + virtual ~ImageYUV422 () throw (); + + inline virtual Encoding + getEncoding () const + { + return (YUV422); + } + + virtual void + fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step = 0) const; + + virtual void + fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step = 0) const; + + virtual bool + isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const; + }; + + } // namespace +} + +#endif // PCL_IO_IMAGE_YUV22_H_ diff --git a/io/include/pcl/io/io_exception.h b/io/include/pcl/io/io_exception.h new file mode 100644 index 00000000000..0586cf480d2 --- /dev/null +++ b/io/include/pcl/io/io_exception.h @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, 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 + +#ifndef PCL_IO_EXCEPTION_H_ +#define PCL_IO_EXCEPTION_H_ + +#include +#include +#include +#include + + +//fom +#if defined _WIN32 && defined _MSC_VER && !defined __PRETTY_FUNCTION__ + #define __PRETTY_FUNCTION__ __FUNCTION__ +#endif + + +#define THROW_IO_EXCEPTION(format,...) throwIOException( __PRETTY_FUNCTION__, __FILE__, __LINE__, format , ##__VA_ARGS__ ) + + +namespace pcl +{ + namespace io + { + /** + * @brief General IO exception class + */ + class IOException : public std::exception + { + public: + IOException (const std::string& function_name, + const std::string& file_name, + unsigned line_number, + const std::string& message); + + virtual ~IOException () throw (); + + IOException& + operator= (const IOException& exception); + + virtual const char* + what () const throw (); + + const std::string& + getFunctionName () const; + + const std::string& + getFileName () const; + + unsigned + getLineNumber () const; + + protected: + std::string function_name_; + std::string file_name_; + unsigned line_number_; + std::string message_; + std::string message_long_; + }; + + inline void + throwIOException (const char* function, const char* file, unsigned line, const char* format, ...) + { + static char msg[1024]; + va_list args; + va_start (args, format); + vsnprintf (msg, 1024, format, args); + throw IOException (function, file, line, msg); + } + } // namespace +} +#endif // PCL_IO_EXCEPTION_H_ diff --git a/io/src/image_depth.cpp b/io/src/image_depth.cpp new file mode 100644 index 00000000000..adb6ff4cb62 --- /dev/null +++ b/io/src/image_depth.cpp @@ -0,0 +1,302 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * Suat Gedikli + * + * 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 Willow Garage, Inc. 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 pcl::io::FrameWrapper; +using pcl::io::IOException; + +pcl::io::DepthImage::DepthImage (FrameWrapper::Ptr depth_metadata, float baseline, float focal_length, uint64_t shadow_value, uint64_t no_sample_value) +: wrapper_ (depth_metadata) +, timestamp_ (Clock::now ()) +, baseline_ (baseline) +, focal_length_ (focal_length) +, shadow_value_ (shadow_value) +, no_sample_value_ (no_sample_value) +{} + + +pcl::io::DepthImage::DepthImage (FrameWrapper::Ptr depth_metadata, float baseline, float focal_length, uint64_t shadow_value, uint64_t no_sample_value, Timestamp timestamp) +: wrapper_(depth_metadata) +, timestamp_(timestamp) +, baseline_ (baseline) +, focal_length_ (focal_length) +, shadow_value_ (shadow_value) +, no_sample_value_ (no_sample_value) +{} + + +pcl::io::DepthImage::~DepthImage () +{} + + +const unsigned short* +pcl::io::DepthImage::getData () +{ + return static_cast (wrapper_->getData ()); +} + + +int +pcl::io::DepthImage::getDataSize () const +{ + return (wrapper_->getDataSize ()); +} + + +const FrameWrapper::Ptr +pcl::io::DepthImage::getMetaData () const +{ + return (wrapper_); +} + + +float +pcl::io::DepthImage::getBaseline () const +{ + return (baseline_); +} + + +float +pcl::io::DepthImage::getFocalLength () const +{ + return (focal_length_); +} + + +uint64_t +pcl::io::DepthImage::getShadowValue () const +{ + return (shadow_value_); +} + + +uint64_t +pcl::io::DepthImage::getNoSampleValue () const +{ + return (no_sample_value_); +} + + +unsigned +pcl::io::DepthImage::getWidth () const +{ + return (wrapper_->getWidth ()); +} + + +unsigned +pcl::io::DepthImage::getHeight () const +{ + return (wrapper_->getHeight ()); +} + + +unsigned +pcl::io::DepthImage::getFrameID () const +{ + return (wrapper_->getFrameID ()); +} + + +uint64_t +pcl::io::DepthImage::getTimestamp () const +{ + return (wrapper_->getTimestamp ()); +} + + +pcl::io::DepthImage::Timestamp +pcl::io::DepthImage::getSystemTimestamp () const +{ + return (timestamp_); +} + +// Fill external buffers //////////////////////////////////////////////////// + +void +pcl::io::DepthImage::fillDepthImageRaw (unsigned width, unsigned height, unsigned short* depth_buffer, unsigned line_step) const +{ + if (width > wrapper_->getWidth () || height > wrapper_->getHeight ()) + THROW_IO_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (wrapper_->getWidth () % width != 0 || wrapper_->getHeight () % height != 0) + THROW_IO_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (line_step == 0) + line_step = width * static_cast (sizeof (unsigned short)); + + // special case no sclaing, no padding => memcopy! + if (width == wrapper_->getWidth () && height == wrapper_->getHeight () && (line_step == width * sizeof (unsigned short))) + { + memcpy (depth_buffer, wrapper_->getData (), wrapper_->getDataSize ()); + return; + } + + // padding skip for destination image + unsigned bufferSkip = line_step - width * static_cast (sizeof (unsigned short)); + + // step and padding skip for source image + unsigned xStep = wrapper_->getWidth () / width; + unsigned ySkip = (wrapper_->getHeight () / height - 1) * wrapper_->getWidth (); + + // Fill in the depth image data, converting mm to m + short bad_point = std::numeric_limits::quiet_NaN (); + unsigned depthIdx = 0; + + const unsigned short* inputBuffer = static_cast (wrapper_->getData ()); + + for (unsigned yIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip) + { + for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++depth_buffer) + { + /// @todo Different values for these cases + unsigned short pixel = inputBuffer[depthIdx]; + if (pixel == 0 || pixel == no_sample_value_ || pixel == shadow_value_) + *depth_buffer = bad_point; + else + { + *depth_buffer = static_cast( pixel ); + } + } + // if we have padding + if (bufferSkip > 0) + { + char* cBuffer = reinterpret_cast (depth_buffer); + depth_buffer = reinterpret_cast (cBuffer + bufferSkip); + } + } +} + +void +pcl::io::DepthImage::fillDepthImage (unsigned width, unsigned height, float* depth_buffer, unsigned line_step) const +{ + if (width > wrapper_->getWidth () || height > wrapper_->getHeight ()) + THROW_IO_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (wrapper_->getWidth () % width != 0 || wrapper_->getHeight () % height != 0) + THROW_IO_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (line_step == 0) + line_step = width * static_cast (sizeof (float)); + + // padding skip for destination image + unsigned bufferSkip = line_step - width * static_cast (sizeof (float)); + + // step and padding skip for source image + unsigned xStep = wrapper_->getWidth () / width; + unsigned ySkip = (wrapper_->getHeight () / height - 1) * wrapper_->getWidth (); + + // Fill in the depth image data, converting mm to m + float bad_point = std::numeric_limits::quiet_NaN (); + unsigned depthIdx = 0; + + const unsigned short* inputBuffer = static_cast (wrapper_->getData ()); + + for (unsigned yIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip) + { + for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++depth_buffer) + { + /// @todo Different values for these cases + unsigned short pixel = inputBuffer[depthIdx]; + if (pixel == 0 || pixel == no_sample_value_ || pixel == shadow_value_) + *depth_buffer = bad_point; + else + { + *depth_buffer = static_cast( pixel ) * 0.001f; // millimeters to meters + } + } + // if we have padding + if (bufferSkip > 0) + { + char* cBuffer = reinterpret_cast (depth_buffer); + depth_buffer = reinterpret_cast (cBuffer + bufferSkip); + } + } +} + +void +pcl::io::DepthImage::fillDisparityImage (unsigned width, unsigned height, float* disparity_buffer, unsigned line_step) const +{ + if (width > wrapper_->getWidth () || height > wrapper_->getHeight ()) + THROW_IO_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (wrapper_->getWidth () % width != 0 || wrapper_->getHeight () % height != 0) + THROW_IO_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (line_step == 0) + line_step = width * static_cast (sizeof (float)); + + unsigned xStep = wrapper_->getWidth () / width; + unsigned ySkip = (wrapper_->getHeight () / height - 1) * wrapper_->getWidth (); + + unsigned bufferSkip = line_step - width * static_cast (sizeof (float)); + + // Fill in the depth image data + // iterate over all elements and fill disparity matrix: disp[x,y] = f * b / z_distance[x,y]; + // focal length is for the native image resolution -> focal_length = focal_length_ / xStep; + float constant = focal_length_ * baseline_ * 1000.0f / static_cast (xStep); + + const unsigned short* inputBuffer = static_cast (wrapper_->getData ()); + + for (unsigned yIdx = 0, depthIdx = 0; yIdx < height; ++yIdx, depthIdx += ySkip) + { + for (unsigned xIdx = 0; xIdx < width; ++xIdx, depthIdx += xStep, ++disparity_buffer) + { + unsigned short pixel = inputBuffer[depthIdx]; + if (pixel == 0 || pixel == no_sample_value_ || pixel == shadow_value_) + *disparity_buffer = 0.0; + else + *disparity_buffer = constant / static_cast (pixel); + } + + // if we have padding + if (bufferSkip > 0) + { + char* cBuffer = reinterpret_cast (disparity_buffer); + disparity_buffer = reinterpret_cast (cBuffer + bufferSkip); + } + } +} diff --git a/io/src/image_ir.cpp b/io/src/image_ir.cpp new file mode 100644 index 00000000000..7080c281ff9 --- /dev/null +++ b/io/src/image_ir.cpp @@ -0,0 +1,160 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, Inc. + * Suat Gedikli + * + * 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 Willow Garage, Inc. 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 pcl::io::FrameWrapper; +using pcl::io::IOException; + +pcl::io::IRImage::IRImage (FrameWrapper::Ptr ir_metadata) + : wrapper_(ir_metadata) + , timestamp_(Clock::now ()) +{} + + +pcl::io::IRImage::IRImage (FrameWrapper::Ptr ir_metadata, Timestamp time) + : wrapper_(ir_metadata) + , timestamp_(time) +{} + + +const unsigned short* +pcl::io::IRImage::getData () +{ + return ( static_cast (wrapper_->getData ()) ); +} + + +int +pcl::io::IRImage::getDataSize () const +{ + return (wrapper_->getDataSize ()); +} + + +const FrameWrapper::Ptr +pcl::io::IRImage::getMetaData () const +{ + return (wrapper_); +} + + +unsigned +pcl::io::IRImage::getWidth () const +{ + return (wrapper_->getWidth ()); +} + + +unsigned +pcl::io::IRImage::getHeight () const +{ + return (wrapper_->getHeight ()); +} + + +unsigned +pcl::io::IRImage::getFrameID () const +{ + return (wrapper_->getFrameID ()); +} + + +uint64_t +pcl::io::IRImage::getTimestamp () const +{ + return (wrapper_->getTimestamp ()); +} + + +pcl::io::IRImage::Timestamp +pcl::io::IRImage::getSystemTimestamp () const +{ + return (timestamp_); +} + + +void pcl::io::IRImage::fillRaw (unsigned width, unsigned height, unsigned short* ir_buffer, unsigned line_step) const +{ + if (width > wrapper_->getWidth () || height > wrapper_->getHeight ()) + THROW_IO_EXCEPTION ("upsampling not supported: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (wrapper_->getWidth () % width != 0 || wrapper_->getHeight () % height != 0) + THROW_IO_EXCEPTION ("downsampling only supported for integer scale: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (line_step == 0) + line_step = width * static_cast (sizeof (unsigned short)); + + // special case no sclaing, no padding => memcopy! + if (width == wrapper_->getWidth () && height == wrapper_->getHeight () && (line_step == width * sizeof (unsigned short))) + { + memcpy (ir_buffer, wrapper_->getData (), wrapper_->getDataSize ()); + return; + } + + // padding skip for destination image + unsigned bufferSkip = line_step - width * static_cast (sizeof (unsigned short)); + + // step and padding skip for source image + unsigned xStep = wrapper_->getWidth () / width; + unsigned ySkip = (wrapper_->getHeight () / height - 1) * wrapper_->getWidth (); + + unsigned irIdx = 0; + + const unsigned short* inputBuffer = static_cast (wrapper_->getData ()); + + for (unsigned yIdx = 0; yIdx < height; ++yIdx, irIdx += ySkip) + { + for (unsigned xIdx = 0; xIdx < width; ++xIdx, irIdx += xStep, ++ir_buffer) + *ir_buffer = inputBuffer[irIdx]; + + // if we have padding + if (bufferSkip > 0) + { + char* cBuffer = reinterpret_cast (ir_buffer); + ir_buffer = reinterpret_cast (cBuffer + bufferSkip); + } + } +} + diff --git a/io/src/image_rgb24.cpp b/io/src/image_rgb24.cpp new file mode 100644 index 00000000000..444457957f0 --- /dev/null +++ b/io/src/image_rgb24.cpp @@ -0,0 +1,160 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, respective authors. + * + * 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 + +using pcl::io::FrameWrapper; +using pcl::io::IOException; + +pcl::io::ImageRGB24::ImageRGB24 (FrameWrapper::Ptr image_metadata) + : Image (image_metadata) +{} + + +pcl::io::ImageRGB24::ImageRGB24 (FrameWrapper::Ptr image_metadata, Timestamp timestamp) + : Image (image_metadata, timestamp) +{} + + +pcl::io::ImageRGB24::~ImageRGB24 () throw () +{} + +bool +pcl::io::ImageRGB24::isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const +{ + return (output_width <= input_width && output_height <= input_height && input_width % output_width == 0 && input_height % output_height == 0 ); +} + + +void +pcl::io::ImageRGB24::fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step) const +{ + if (width > wrapper_->getWidth () || height > wrapper_->getHeight ()) + THROW_IO_EXCEPTION ("Up-sampling not supported. Request was %d x %d -> %d x %d.", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (wrapper_->getWidth () % width == 0 && wrapper_->getHeight () % height == 0) + { + unsigned src_step = wrapper_->getWidth () / width; + unsigned src_skip = (wrapper_->getHeight () / height - 1) * wrapper_->getWidth (); + + if (gray_line_step == 0) + gray_line_step = width; + + unsigned dst_skip = gray_line_step - width; // skip of padding values in bytes + + unsigned char* dst_line = gray_buffer; + const RGB888Pixel* src_line = (RGB888Pixel*) wrapper_->getData (); + + for (unsigned yIdx = 0; yIdx < height; ++yIdx, src_line += src_skip, dst_line += dst_skip) + { + for (unsigned xIdx = 0; xIdx < width; ++xIdx, src_line += src_step, dst_line ++) + { + *dst_line = static_cast((static_cast (src_line->r) * 299 + + static_cast (src_line->g) * 587 + + static_cast (src_line->b) * 114) * 0.001); + } + } + } + else + { + THROW_IO_EXCEPTION ("Down-sampling only possible for integer scale. Request was %d x %d -> %d x %d.", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + } +} + + +void +pcl::io::ImageRGB24::fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step) const +{ + if (width > wrapper_->getWidth () || height > wrapper_->getHeight ()) + THROW_IO_EXCEPTION ("Up-sampling not supported. Request was %d x %d -> %d x %d.", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (width == wrapper_->getWidth () && height == wrapper_->getHeight ()) + { + unsigned line_size = width * 3; + if (rgb_line_step == 0 || rgb_line_step == line_size) + { + memcpy (rgb_buffer, wrapper_->getData (), wrapper_->getDataSize ()); + } + else // line by line + { + unsigned char* rgb_line = rgb_buffer; + const unsigned char* src_line = static_cast (wrapper_->getData ()); + for (unsigned yIdx = 0; yIdx < height; ++yIdx, rgb_line += rgb_line_step, src_line += line_size) + { + memcpy (rgb_line, src_line, line_size); + } + } + } + else if (wrapper_->getWidth () % width == 0 && wrapper_->getHeight () % height == 0) // downsamplig + { + unsigned src_step = wrapper_->getWidth () / width; + unsigned src_skip = (wrapper_->getHeight () / height - 1) * wrapper_->getWidth (); + + if (rgb_line_step == 0) + rgb_line_step = width * 3; + + unsigned dst_skip = rgb_line_step - width * 3; // skip of padding values in bytes + + RGB888Pixel* dst_line = reinterpret_cast (rgb_buffer); + const RGB888Pixel* src_line = (RGB888Pixel*) wrapper_->getData (); + + for (unsigned yIdx = 0; yIdx < height; ++yIdx, src_line += src_skip) + { + for (unsigned xIdx = 0; xIdx < width; ++xIdx, src_line += src_step, dst_line ++) + { + *dst_line = *src_line; + } + + if (dst_skip != 0) + { + // use bytes to skip rather than XnRGB24Pixel's, since line_step does not need to be multiple of 3 + unsigned char* temp = reinterpret_cast (dst_line); + dst_line = reinterpret_cast (temp + dst_skip); + } + } + } + else + { + THROW_IO_EXCEPTION ("Down-sampling only possible for integer scale. Request was %d x %d -> %d x %d.", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + } +} + diff --git a/io/src/image_yuv422.cpp b/io/src/image_yuv422.cpp new file mode 100644 index 00000000000..3e1b69a1fbf --- /dev/null +++ b/io/src/image_yuv422.cpp @@ -0,0 +1,160 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 2011 Willow Garage, Inc. + * Suat Gedikli + * + * 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 Willow Garage, Inc. 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 + +#define CLIP_CHAR(c) static_cast ((c)>255?255:(c)<0?0:(c)) + +using pcl::io::FrameWrapper; +using pcl::io::IOException; + +pcl::io::ImageYUV422::ImageYUV422 (FrameWrapper::Ptr image_metadata) + : Image (image_metadata) +{} + + +pcl::io::ImageYUV422::ImageYUV422 (FrameWrapper::Ptr image_metadata, Timestamp timestamp) + : Image (image_metadata, timestamp) +{} + + +pcl::io::ImageYUV422::~ImageYUV422 () throw () +{} + +bool +pcl::io::ImageYUV422::isResizingSupported (unsigned input_width, unsigned input_height, unsigned output_width, unsigned output_height) const +{ + return (output_width <= input_width && output_height <= input_height && input_width % output_width == 0 && input_height % output_height == 0 ); +} + + +void +pcl::io::ImageYUV422::fillRGB (unsigned width, unsigned height, unsigned char* rgb_buffer, unsigned rgb_line_step) const +{ + // 0 1 2 3 + // u y1 v y2 + + if (wrapper_->getWidth () != width && wrapper_->getHeight () != height) + { + if (width > wrapper_->getWidth () || height > wrapper_->getHeight () ) + THROW_IO_EXCEPTION ("Upsampling not supported. Request was: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if ( wrapper_->getWidth () % width != 0 || wrapper_->getHeight () % height != 0 + || (wrapper_->getWidth () / width) & 0x01 || (wrapper_->getHeight () / height & 0x01) ) + THROW_IO_EXCEPTION ("Downsampling only possible for power of two scale in both dimensions. Request was %d x %d -> %d x %d.", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + } + + register const uint8_t* yuv_buffer = (uint8_t*) wrapper_->getData (); + + unsigned rgb_line_skip = 0; + if (rgb_line_step != 0) + rgb_line_skip = rgb_line_step - width * 3; + + if (wrapper_->getWidth () == width && wrapper_->getHeight () == height) + { + for ( register unsigned yIdx = 0; yIdx < height; ++yIdx, rgb_buffer += rgb_line_skip ) + { + for ( register unsigned xIdx = 0; xIdx < width; xIdx += 2, rgb_buffer += 6, yuv_buffer += 4 ) + { + int v = yuv_buffer[2] - 128; + int u = yuv_buffer[0] - 128; + + rgb_buffer[0] = CLIP_CHAR (yuv_buffer[1] + ((v * 18678 + 8192 ) >> 14)); + rgb_buffer[1] = CLIP_CHAR (yuv_buffer[1] + ((v * -9519 - u * 6472 + 8192 ) >> 14)); + rgb_buffer[2] = CLIP_CHAR (yuv_buffer[1] + ((u * 33292 + 8192 ) >> 14)); + + rgb_buffer[3] = CLIP_CHAR (yuv_buffer[3] + ((v * 18678 + 8192 ) >> 14)); + rgb_buffer[4] = CLIP_CHAR (yuv_buffer[3] + ((v * -9519 - u * 6472 + 8192 ) >> 14)); + rgb_buffer[5] = CLIP_CHAR (yuv_buffer[3] + ((u * 33292 + 8192 ) >> 14)); + } + } + } + else + { + register unsigned yuv_step = wrapper_->getWidth () / width; + register unsigned yuv_x_step = yuv_step << 1; + register unsigned yuv_skip = (wrapper_->getHeight () / height - 1) * ( wrapper_->getWidth () << 1 ); + + for ( register unsigned yIdx = 0; yIdx < wrapper_->getHeight (); yIdx += yuv_step, yuv_buffer += yuv_skip, rgb_buffer += rgb_line_skip ) + { + for ( register unsigned xIdx = 0; xIdx < wrapper_->getWidth (); xIdx += yuv_step, rgb_buffer += 3, yuv_buffer += yuv_x_step ) + { + int v = yuv_buffer[2] - 128; + int u = yuv_buffer[0] - 128; + + rgb_buffer[0] = CLIP_CHAR (yuv_buffer[1] + ((v * 18678 + 8192 ) >> 14)); + rgb_buffer[1] = CLIP_CHAR (yuv_buffer[1] + ((v * -9519 - u * 6472 + 8192 ) >> 14)); + rgb_buffer[2] = CLIP_CHAR (yuv_buffer[1] + ((u * 33292 + 8192 ) >> 14)); + } + } + } +} + + +void +pcl::io::ImageYUV422::fillGrayscale (unsigned width, unsigned height, unsigned char* gray_buffer, unsigned gray_line_step) const +{ + // u y1 v y2 + if (width > wrapper_->getWidth () || height > wrapper_->getHeight ()) + THROW_IO_EXCEPTION ("Upsampling not supported. Request was: %d x %d -> %d x %d", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + if (wrapper_->getWidth () % width != 0 || wrapper_->getHeight () % height != 0) + THROW_IO_EXCEPTION ("Downsampling only possible for integer scales in both dimensions. Request was %d x %d -> %d x %d.", wrapper_->getWidth (), wrapper_->getHeight (), width, height); + + unsigned gray_line_skip = 0; + if (gray_line_step != 0) + gray_line_skip = gray_line_step - width; + + register unsigned yuv_step = wrapper_->getWidth () / width; + register unsigned yuv_x_step = yuv_step << 1; + register unsigned yuv_skip = (wrapper_->getHeight () / height - 1) * ( wrapper_->getWidth () << 1 ); + register const uint8_t* yuv_buffer = ( (uint8_t*) wrapper_->getData () + 1); + + for ( register unsigned yIdx = 0; yIdx < wrapper_->getHeight (); yIdx += yuv_step, yuv_buffer += yuv_skip, gray_buffer += gray_line_skip ) + { + for ( register unsigned xIdx = 0; xIdx < wrapper_->getWidth (); xIdx += yuv_step, ++gray_buffer, yuv_buffer += yuv_x_step ) + { + *gray_buffer = *yuv_buffer; + } + } +} + diff --git a/io/src/io_exception.cpp b/io/src/io_exception.cpp new file mode 100644 index 00000000000..614ef1b6db5 --- /dev/null +++ b/io/src/io_exception.cpp @@ -0,0 +1,83 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011 Willow Garage, 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 Willow Garage, Inc. 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 "pcl/io/io_exception.h" +#include + +pcl::io::IOException::IOException (const std::string& function_name, const std::string& file_name, unsigned line_number, const std::string& message) + : function_name_ (function_name) + , file_name_ (file_name) + , line_number_ (line_number) + , message_ (message) +{ + std::stringstream sstream; + sstream << function_name_ << " @ " << file_name_ << " @ " << line_number_ << " : " << message_; + message_long_ = sstream.str (); +} + +pcl::io::IOException::~IOException () throw () +{ +} + +pcl::io::IOException& +pcl::io::IOException::operator = (const IOException& exception) +{ + message_ = exception.message_; + return (*this); +} + +const char* +pcl::io::IOException::what () const throw () +{ + return (message_long_.c_str ()); +} + +const std::string& +pcl::io::IOException::getFunctionName () const +{ + return (function_name_); +} + +const std::string& +pcl::io::IOException::getFileName () const +{ + return (file_name_); +} + +unsigned +pcl::io::IOException::getLineNumber () const +{ + return (line_number_); +} From 50b50afedb5ed3dc4a1ac1efc531a0d7e1289f01 Mon Sep 17 00:00:00 2001 From: Ky Waegel Date: Sun, 16 Feb 2014 23:34:11 -0500 Subject: [PATCH 05/12] Added OpenNI 2 grabber. --- io/CMakeLists.txt | 118 ++- io/include/pcl/io/openni2/openni.h | 87 ++ io/include/pcl/io/openni2/openni2_convert.h | 65 ++ io/include/pcl/io/openni2/openni2_device.h | 303 ++++++ .../pcl/io/openni2/openni2_device_info.h | 62 ++ .../pcl/io/openni2/openni2_device_manager.h | 102 ++ .../pcl/io/openni2/openni2_frame_listener.h | 89 ++ .../pcl/io/openni2/openni2_metadata_wrapper.h | 116 +++ .../pcl/io/openni2/openni2_timer_filter.h | 74 ++ .../pcl/io/openni2/openni2_video_mode.h | 93 ++ .../openni_shift_to_depth_conversion.h | 124 +++ io/include/pcl/io/openni2_grabber.h | 505 +++++++++ io/src/openni2/openni2_convert.cpp | 107 ++ io/src/openni2/openni2_device.cpp | 903 +++++++++++++++++ io/src/openni2/openni2_device_info.cpp | 54 + io/src/openni2/openni2_device_manager.cpp | 266 +++++ io/src/openni2/openni2_timer_filter.cpp | 98 ++ io/src/openni2/openni2_video_mode.cpp | 102 ++ io/src/openni2_grabber.cpp | 954 ++++++++++++++++++ 19 files changed, 4195 insertions(+), 27 deletions(-) create mode 100644 io/include/pcl/io/openni2/openni.h create mode 100644 io/include/pcl/io/openni2/openni2_convert.h create mode 100644 io/include/pcl/io/openni2/openni2_device.h create mode 100644 io/include/pcl/io/openni2/openni2_device_info.h create mode 100644 io/include/pcl/io/openni2/openni2_device_manager.h create mode 100644 io/include/pcl/io/openni2/openni2_frame_listener.h create mode 100644 io/include/pcl/io/openni2/openni2_metadata_wrapper.h create mode 100644 io/include/pcl/io/openni2/openni2_timer_filter.h create mode 100644 io/include/pcl/io/openni2/openni2_video_mode.h create mode 100644 io/include/pcl/io/openni2/openni_shift_to_depth_conversion.h create mode 100644 io/include/pcl/io/openni2_grabber.h create mode 100644 io/src/openni2/openni2_convert.cpp create mode 100644 io/src/openni2/openni2_device.cpp create mode 100644 io/src/openni2/openni2_device_info.cpp create mode 100644 io/src/openni2/openni2_device_manager.cpp create mode 100644 io/src/openni2/openni2_timer_filter.cpp create mode 100644 io/src/openni2/openni2_video_mode.cpp create mode 100644 io/src/openni2_grabber.cpp diff --git a/io/CMakeLists.txt b/io/CMakeLists.txt index d42c7e90f3a..3d0fd82c227 100644 --- a/io/CMakeLists.txt +++ b/io/CMakeLists.txt @@ -5,9 +5,9 @@ set(SUBSYS_DEPS common octree) set(build TRUE) PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) if(WIN32) - PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni pcap png vtk) + PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni openni2 pcap png vtk) else(WIN32) - PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni pcap png vtk libusb-1.0) + PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} OPT_DEPS openni openni2 pcap png vtk libusb-1.0) endif(WIN32) PCL_ADD_DOC("${SUBSYS_NAME}") @@ -22,7 +22,7 @@ if(build) include/pcl/io/image_depth.h include/pcl/io/io_exception.h ) - + set(IMAGE_SOURCES src/image_rgb24.cpp src/image_yuv422.cpp @@ -30,13 +30,59 @@ if(build) src/image_depth.cpp src/io_exception.cpp ) - + + ## OpenNI 2.x + OPTION(BUILD_OPENNI2 "Build the OpenNI 2 Grabber." OFF) + MARK_AS_ADVANCED(BUILD_OPENNI2) + if(NOT BUILD_OPENNI2) + # Set OPENNI2_FOUND to false locally to avoid building anything OpenNI 2 related + # Remember that other modules (libraries) need to check explicitly for BUILD_OPENNI2 + set(OPENNI2_FOUND FALSE) + endif() + if(OPENNI2_FOUND) + set(OPENNI2_GRABBER_INCLUDES + include/pcl/io/openni2_grabber.h + ) + set(OPENNI2_INCLUDES + include/pcl/io/openni2/openni.h + include/pcl/io/openni2/openni2_metadata_wrapper.h + include/pcl/io/openni2/openni2_frame_listener.h + include/pcl/io/openni2/openni2_timer_filter.h + include/pcl/io/openni2/openni2_video_mode.h + include/pcl/io/openni2/openni2_convert.h + include/pcl/io/openni2/openni2_device.h + include/pcl/io/openni2/openni2_device_info.h + include/pcl/io/openni2/openni2_device_manager.h + ${IMAGE_INCLUDES} + ) + + set(OPENNI2_GRABBER_SOURCES + src/openni2_grabber.cpp + src/openni2/openni2_timer_filter.cpp + src/openni2/openni2_video_mode.cpp + src/openni2/openni2_convert.cpp + src/openni2/openni2_device.cpp + src/openni2/openni2_device_info.cpp + src/openni2/openni2_device_manager.cpp + ${IMAGE_SOURCES} + ) + + source_group("OpenNI 2\\Header Files" FILES ${OPENNI2_GRABBER_INCLUDES} ${OPENNI2_INCLUDES}) + source_group("OpenNI 2\\Source Files" FILES ${OPENNI2_GRABBER_SOURCES}) + + # Copy OpenNI2 redist directory to bin. Needed for driver modules. Only tested on Windows. + if(MSVC) + file(COPY ${OPENNI2_REDIST_DIR} DESTINATION ${CMAKE_BINARY_DIR}/bin PATTERN *.*) + endif(MSVC) + endif(OPENNI2_FOUND) + + ## OpenNI 1.x OPTION(BUILD_OPENNI "Build the OpenNI Grabber." ON) MARK_AS_ADVANCED(BUILD_OPENNI) if(NOT BUILD_OPENNI) # Set OPENNI_FOUND to false locally to avoid building anything OpenNI related # Remember that other modules (libraries) need to check explicitly for BUILD_OPENNI - set(OPENNI_FOUND FALSE) + set(OPENNI_FOUND FALSE) endif() if(OPENNI_FOUND) set(OPENNI_GRABBER_INCLUDES @@ -59,8 +105,10 @@ if(build) include/pcl/io/openni_camera/openni_image_yuv_422.h include/pcl/io/openni_camera/openni_image_rgb24.h include/pcl/io/openni_camera/openni_ir_image.h - ) - set(OPENNI_GRABBER_SOURCES src/openni_camera/openni_device.cpp + ${IMAGE_INCLUDES} + ) + set(OPENNI_GRABBER_SOURCES + src/openni_camera/openni_device.cpp src/openni_camera/openni_device_primesense.cpp src/openni_camera/openni_image_bayer_grbg.cpp src/openni_camera/openni_depth_image.cpp @@ -74,7 +122,8 @@ if(build) src/openni_camera/openni_image_rgb24.cpp src/openni_grabber.cpp src/oni_grabber.cpp - ) + ${IMAGE_SOURCES} + ) endif(OPENNI_FOUND) source_group("Image Headers" FILES ${IMAGE_INCLUDES}) @@ -87,11 +136,11 @@ if(build) # set(FZAPI_INCLUDES # include/pcl/io/openni_camera/openni.h # ) - set(FZAPI_GRABBER_SOURCES + set(FZAPI_GRABBER_SOURCES src/fotonic_grabber.cpp ) endif(FZAPI_FOUND) - + if(PXCAPI_FOUND) set(PXC_GRABBER_INCLUDES include/pcl/io/pxc_grabber.h @@ -100,7 +149,7 @@ if(build) src/pxc_grabber.cpp ) endif(PXCAPI_FOUND) - + if(LIBUSB_1_FOUND) set(DINAST_GRABBER_INCLUDES include/pcl/io/dinast_grabber.h @@ -113,7 +162,7 @@ if(build) if (VTK_FOUND AND NOT ANDROID) set(VTK_USE_FILE "${VTK_USE_FILE}" CACHE INTERNAL "VTK_USE_FILE") include("${VTK_USE_FILE}") - set(VTK_IO_INCLUDES + set(VTK_IO_INCLUDES "include/pcl/${SUBSYS_NAME}/vtk_lib_io.h" "include/pcl/${SUBSYS_NAME}/png_io.h" ) @@ -121,7 +170,7 @@ if(build) "include/pcl/${SUBSYS_NAME}/impl/vtk_lib_io.hpp" "include/pcl/${SUBSYS_NAME}/impl/png_io.hpp" ) - set(VTK_IO_SOURCE + set(VTK_IO_SOURCE src/vtk_lib_io.cpp src/png_io.cpp ) @@ -131,7 +180,7 @@ if(build) endif () set(PLY_SOURCES src/ply/ply_parser.cpp) - set(PLY_INCLUDES + set(PLY_INCLUDES "include/pcl/${SUBSYS_NAME}/ply/byte_order.h" "include/pcl/${SUBSYS_NAME}/ply/io_operators.h" "include/pcl/${SUBSYS_NAME}/ply/ply.h" @@ -140,7 +189,7 @@ if(build) PCL_ADD_LIBRARY(pcl_io_ply "${SUBSYS_NAME}" ${PLY_SOURCES} ${PLY_INCLUDES}) PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/ply" ${PLY_INCLUDES}) - set(srcs + set(srcs src/debayer.cpp src/pcd_grabber.cpp src/pcd_io.cpp @@ -157,6 +206,7 @@ if(build) src/robot_eye_grabber.cpp ${VTK_IO_SOURCE} ${OPENNI_GRABBER_SOURCES} + ${OPENNI2_GRABBER_SOURCES} ${DINAST_GRABBER_SOURCES} ${FZAPI_GRABBER_SOURCES} ${PXC_GRABBER_SOURCES} @@ -174,7 +224,7 @@ if(build) add_definitions(${PCAP_DEFINES}) endif(PCAP_FOUND) - set(incs + set(incs "include/pcl/${SUBSYS_NAME}/boost.h" "include/pcl/${SUBSYS_NAME}/eigen.h" "include/pcl/${SUBSYS_NAME}/debayer.h" @@ -198,9 +248,10 @@ if(build) "include/pcl/${SUBSYS_NAME}/point_cloud_image_extractors.h" ${VTK_IO_INCLUDES} ${OPENNI_GRABBER_INCLUDES} + ${OPENNI2_GRABBER_INCLUDES} ${DINAST_GRABBER_INCLUDES} ${FZAPI_GRABBER_INCLUDES} - ${PXC_GRABBER_INCLUDES} + ${PXC_GRABBER_INCLUDES} ) set(compression_incs @@ -215,14 +266,14 @@ if(build) include/pcl/compression/organized_pointcloud_conversion.h include/pcl/compression/libpng_wrapper.h ) - if(OPENNI_FOUND) + if(OPENNI_FOUND OR OPENNI2_FOUND) list(APPEND compression_incs include/pcl/compression/organized_pointcloud_compression.h ) - endif(OPENNI_FOUND) + endif(OPENNI_FOUND OR OPENNI2_FOUND) endif(PNG_FOUND) - set(impl_incs + set(impl_incs "include/pcl/${SUBSYS_NAME}/impl/pcd_io.hpp" "include/pcl/${SUBSYS_NAME}/impl/lzf_image_io.hpp" "include/pcl/${SUBSYS_NAME}/impl/synchronized_queue.hpp" @@ -231,17 +282,17 @@ if(build) include/pcl/compression/impl/octree_pointcloud_compression.hpp ${VTK_IO_INCLUDES_IMPL} ) - if(PNG_FOUND AND OPENNI_FOUND) + if(PNG_FOUND AND (OPENNI_FOUND OR OPENNI2_FOUND) ) list(APPEND impl_incs include/pcl/compression/impl/organized_pointcloud_compression.hpp ) - endif(PNG_FOUND AND OPENNI_FOUND) + endif(PNG_FOUND AND (OPENNI_FOUND OR OPENNI2_FOUND) ) set(LIB_NAME "pcl_${SUBSYS_NAME}") include_directories("${CMAKE_CURRENT_SOURCE_DIR}/include" ${VTK_INCLUDE_DIRECTORIES}) add_definitions(${VTK_DEFINES}) - PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${compression_incs} ${impl_incs} ${OPENNI_INCLUDES}) + PCL_ADD_LIBRARY("${LIB_NAME}" "${SUBSYS_NAME}" ${srcs} ${incs} ${compression_incs} ${impl_incs} ${OPENNI_INCLUDES} ${OPENNI2_INCLUDES}) link_directories(${VTK_LINK_DIRECTORIES}) target_link_libraries("${LIB_NAME}" pcl_common pcl_io_ply ${VTK_IO_TARGET_LINK_LIBRARIES} ) if(PNG_FOUND) @@ -252,27 +303,39 @@ if(build) target_link_libraries("${LIB_NAME}" ${LIBUSB_1_LIBRARIES}) endif(LIBUSB_1_FOUND) + if(OPENNI2_FOUND) + target_link_libraries(${LIB_NAME} ${OPENNI2_LIBRARIES}) + endif(OPENNI2_FOUND) + if(OPENNI_FOUND) target_link_libraries("${LIB_NAME}" ${OPENNI_LIBRARIES}) endif(OPENNI_FOUND) - + if(FZAPI_FOUND) target_link_libraries("${LIB_NAME}" ${FZAPI_LIBS}) if(WIN32) target_link_libraries("${LIB_NAME}" Version.lib) endif(WIN32) endif(FZAPI_FOUND) - + if(PXCAPI_FOUND) link_directories(${PXCAPI_LIB_DIRS}) target_link_libraries("${LIB_NAME}" ${PXCAPI_LIBS}) endif(PXCAPI_FOUND) - + if (PCAP_FOUND) target_link_libraries("${LIB_NAME}" ${PCAP_LIBRARIES}) endif(PCAP_FOUND) set(EXT_DEPS eigen3) + + if(OPENNI_FOUND) + list(APPEND EXT_DEPS openni-dev) + endif(OPENNI_FOUND) + if(OPENNI2_FOUND) + list(APPEND EXT_DEPS openni2-dev) + endif(OPENNI2_FOUND) + PCL_MAKE_PKGCONFIG("${LIB_NAME}" "${SUBSYS_NAME}" "${SUBSYS_DESC}" "${SUBSYS_DEPS}" "${EXT_DEPS}" "" "" "") @@ -280,8 +343,9 @@ if(build) PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) PCL_ADD_INCLUDES("${SUBSYS_NAME}" compression ${compression_incs}) PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/openni_camera" ${OPENNI_INCLUDES}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/openni2_camera" ${OPENNI2_INCLUDES}) PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) - + add_subdirectory(tools) endif(build) diff --git a/io/include/pcl/io/openni2/openni.h b/io/include/pcl/io/openni2/openni.h new file mode 100644 index 00000000000..b9f43dea345 --- /dev/null +++ b/io/include/pcl/io/openni2/openni.h @@ -0,0 +1,87 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, 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 +#ifdef HAVE_OPENNI2 + +#ifndef PCL_IO_OPENNI2_OPENNI_H_ +#define PCL_IO_OPENNI2_OPENNI_H_ + +#if defined __GNUC__ +# pragma GCC system_header +#endif + +#include +#include + +// Standard resolutions, ported from OpenNI 1.x. To be removed later. +#define XN_QQVGA_X_RES 160 +#define XN_QQVGA_Y_RES 120 +#define XN_CGA_X_RES 320 +#define XN_CGA_Y_RES 200 +#define XN_QVGA_X_RES 320 +#define XN_QVGA_Y_RES 240 +#define XN_VGA_X_RES 640 +#define XN_VGA_Y_RES 480 +#define XN_SVGA_X_RES 800 +#define XN_SVGA_Y_RES 600 +#define XN_XGA_X_RES 1024 +#define XN_XGA_Y_RES 768 +#define XN_720P_X_RES 1280 +#define XN_720P_Y_RES 720 +#define XN_SXGA_X_RES 1280 +#define XN_SXGA_Y_RES 1024 +#define XN_UXGA_X_RES 1600 +#define XN_UXGA_Y_RES 1200 +#define XN_1080P_X_RES 1920 +#define XN_1080P_Y_RES 1080 +#define XN_QCIF_X_RES 176 +#define XN_QCIF_Y_RES 144 +#define XN_240P_X_RES 423 +#define XN_240P_Y_RES 240 +#define XN_CIF_X_RES 352 +#define XN_CIF_Y_RES 288 +#define XN_WVGA_X_RES 640 +#define XN_WVGA_Y_RES 360 +#define XN_480P_X_RES 864 +#define XN_480P_Y_RES 480 +#define XN_576P_X_RES 1024 +#define XN_576P_Y_RES 576 +#define XN_DV_X_RES 960 +#define XN_DV_Y_RES 720 + +#endif // PCL_IO_OPENNI2_OPENNI_H_ +#endif // HAVE_OPENNI2 diff --git a/io/include/pcl/io/openni2/openni2_convert.h b/io/include/pcl/io/openni2/openni2_convert.h new file mode 100644 index 00000000000..bfc439e31f3 --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_convert.h @@ -0,0 +1,65 @@ +/* + * Copyright (c) 2013, Willow Garage, 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 Willow Garage, Inc. 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. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + + +#ifndef PCL_IO_OPENNI2_CONVERT_H_ +#define PCL_IO_OPENNI2_CONVERT_H_ + +#include "pcl/io/openni2/openni2_device_info.h" +#include "pcl/io/openni2/openni2_video_mode.h" + +#include "OpenNI.h" + +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + const OpenNI2DeviceInfo + openni2_convert (const openni::DeviceInfo* pInfo); + + const openni::VideoMode + grabberModeToOpenniMode (const OpenNI2VideoMode& input); + + const OpenNI2VideoMode + openniModeToGrabberMode (const openni::VideoMode& input); + + const std::vector + openniModeToGrabberMode (const openni::Array& input); + + } // namespace + } +} + +#endif // PCL_IO_OPENNI2_CONVERT_H_ diff --git a/io/include/pcl/io/openni2/openni2_device.h b/io/include/pcl/io/openni2/openni2_device.h new file mode 100644 index 00000000000..6ffa8b1fb41 --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_device.h @@ -0,0 +1,303 @@ +/* + * Copyright (c) 2013, Willow Garage, 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 Willow Garage, Inc. 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_OPENNI2_DEVICE_H_ +#define PCL_IO_OPENNI2_DEVICE_H_ + +#include +#include "openni.h" +#include "pcl/io/openni2/openni2_video_mode.h" +#include "pcl/io/io_exception.h" + +#include +#include +#include +#include +#include +#include + +// Template frame wrappers +#include +#include +#include + + + +namespace openni +{ + class Device; + class DeviceInfo; + class VideoStream; + class SensorInfo; +} + +namespace pcl +{ + namespace io + { + namespace openni2 + { + typedef pcl::io::DepthImage DepthImage; + typedef pcl::io::IRImage IRImage; + typedef pcl::io::Image Image; + + class OpenNI2FrameListener; + + class PCL_EXPORTS OpenNI2Device + { + public: + + typedef boost::function, void* cookie) > ImageCallbackFunction; + typedef boost::function, void* cookie) > DepthImageCallbackFunction; + typedef boost::function, void* cookie) > IRImageCallbackFunction; + typedef unsigned CallbackHandle; + + typedef boost::function StreamCallbackFunction; + + OpenNI2Device (const std::string& device_URI); + virtual ~OpenNI2Device (); + + const std::string + getUri () const; + const std::string + getVendor () const; + const std::string + getName () const; + uint16_t + getUsbVendorId () const; + uint16_t + getUsbProductId () const; + + const std::string + getStringID () const; + + bool + isValid () const; + + bool + hasIRSensor () const; + bool + hasColorSensor () const; + bool + hasDepthSensor () const; + + void + startIRStream (); + void + startColorStream (); + void + startDepthStream (); + + void + stopAllStreams (); + + void + stopIRStream (); + void + stopColorStream (); + void + stopDepthStream (); + + bool + isIRStreamStarted (); + bool + isColorStreamStarted (); + bool + isDepthStreamStarted (); + + bool + isImageRegistrationModeSupported () const; + void + setImageRegistrationMode (bool enabled); + bool + isDepthRegistered () const; + + const OpenNI2VideoMode + getIRVideoMode (); + const OpenNI2VideoMode + getColorVideoMode (); + const OpenNI2VideoMode + getDepthVideoMode (); + + const std::vector& + getSupportedIRVideoModes () const; + const std::vector& + getSupportedColorVideoModes () const; + const std::vector& + getSupportedDepthVideoModes () const; + + bool + isIRVideoModeSupported (const OpenNI2VideoMode& video_mode) const; + bool + isColorVideoModeSupported (const OpenNI2VideoMode& video_mode) const; + bool + isDepthVideoModeSupported (const OpenNI2VideoMode& video_mode) const; + + bool + findCompatibleIRMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const; + bool + findCompatibleColorMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const; + bool + findCompatibleDepthMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const; + + void + setIRVideoMode (const OpenNI2VideoMode& video_mode); + void + setColorVideoMode (const OpenNI2VideoMode& video_mode); + void + setDepthVideoMode (const OpenNI2VideoMode& video_mode); + + OpenNI2VideoMode + getDefaultIRMode () const; + OpenNI2VideoMode + getDefaultColorMode () const; + OpenNI2VideoMode + getDefaultDepthMode () const; + + float + getIRFocalLength (int output_y_resolution) const; + float + getColorFocalLength (int output_y_resolution) const; + float + getDepthFocalLength (int output_y_resolution) const; + + // Baseline between sensors. Returns 0 if this value does not exist. + float + getBaseline(); + + // Value of pixels in shadow or that have no valid measurement + uint64_t + getShadowValue(); + + void + setAutoExposure (bool enable); + void + setAutoWhiteBalance (bool enable); + + inline bool + isSynchronized () + { + return (openni_device_->getDepthColorSyncEnabled ()); + } + + inline bool + isSynchronizationSupported () + { + return (true); // Not sure how to query this from the hardware + } + + inline bool + isFile() + { + return (openni_device_->isFile()); + } + + void + setSynchronization (bool enableSync); + + bool + getAutoExposure () const; + bool + getAutoWhiteBalance () const; + + void + setUseDeviceTimer (bool enable); + + /************************************************************************************/ + // Callbacks from openni::VideoStream to grabber. Internal interface + void + setColorCallback (StreamCallbackFunction color_callback); + void + setDepthCallback (StreamCallbackFunction depth_callback); + void + setIRCallback (StreamCallbackFunction ir_callback); + + protected: + void shutdown (); + + boost::shared_ptr + getIRVideoStream () const; + boost::shared_ptr + getColorVideoStream () const; + boost::shared_ptr + getDepthVideoStream () const; + + + void + processColorFrame (openni::VideoStream& stream); + void + processDepthFrame (openni::VideoStream& stream); + void + processIRFrame (openni::VideoStream& stream); + + + bool + findCompatibleVideoMode (const std::vector supportedModes, + const OpenNI2VideoMode& output_mode, OpenNI2VideoMode& mode) const; + + bool + resizingSupported (size_t input_width, size_t input_height, size_t output_width, size_t output_height) const; + + // Members + + boost::shared_ptr openni_device_; + boost::shared_ptr device_info_; + + boost::shared_ptr ir_frame_listener; + boost::shared_ptr color_frame_listener; + boost::shared_ptr depth_frame_listener; + + mutable boost::shared_ptr ir_video_stream_; + mutable boost::shared_ptr color_video_stream_; + mutable boost::shared_ptr depth_video_stream_; + + mutable std::vector ir_video_modes_; + mutable std::vector color_video_modes_; + mutable std::vector depth_video_modes_; + + bool ir_video_started_; + bool color_video_started_; + bool depth_video_started_; + + /** \brief distance between the projector and the IR camera in meters*/ + float baseline_; + /** the value for shadow (occluded pixels) */ + uint64_t shadow_value_; + /** the value for pixels without a valid disparity measurement */ + uint64_t no_sample_value_; + }; + + PCL_EXPORTS std::ostream& operator<< (std::ostream& stream, const OpenNI2Device& device); + + } // namespace + } +} + +#endif // PCL_IO_OPENNI2_DEVICE_H_ diff --git a/io/include/pcl/io/openni2/openni2_device_info.h b/io/include/pcl/io/openni2/openni2_device_info.h new file mode 100644 index 00000000000..2954e45e1a2 --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_device_info.h @@ -0,0 +1,62 @@ +/* + * Copyright (c) 2013, Willow Garage, 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 Willow Garage, Inc. 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. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#ifndef PCL_IO_OPENNI2_DEVICE_INFO_H_ +#define PCL_IO_OPENNI2_DEVICE_INFO_H_ + +#include + +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + struct OpenNI2DeviceInfo + { + std::string uri_; + std::string vendor_; + std::string name_; + uint16_t vendor_id_; + uint16_t product_id_; + }; + + std::ostream& + operator<< (std::ostream& stream, const OpenNI2DeviceInfo& device_info); + + } // namespace + } +} + +#endif // PCL_IO_OPENNI2_DEVICE_INFO_H_ diff --git a/io/include/pcl/io/openni2/openni2_device_manager.h b/io/include/pcl/io/openni2/openni2_device_manager.h new file mode 100644 index 00000000000..5e6d623a442 --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_device_manager.h @@ -0,0 +1,102 @@ +/* + * Copyright (c) 2013, Willow Garage, 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 Willow Garage, Inc. 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. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#ifndef PCL_IO_OPENNI2_DEVICE_MANAGER_H_ +#define PCL_IO_OPENNI2_DEVICE_MANAGER_H_ + +#include +#include "pcl/io/openni2/openni2_device_info.h" + +#include +#include +#include + +#include +#include +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + class OpenNI2DeviceListener; + class OpenNI2Device; + + class PCL_EXPORTS OpenNI2DeviceManager + { + public: + OpenNI2DeviceManager (); + virtual ~OpenNI2DeviceManager (); + + // This may not actually be a sigleton yet. Need to work out cross-dll incerface. + // Based on http://stackoverflow.com/a/13431981/1789618 + static boost::shared_ptr getInstance () + { + static boost::shared_ptr instance = boost::make_shared(); + return (instance); + } + + boost::shared_ptr > + getConnectedDeviceInfos () const; + + boost::shared_ptr > + getConnectedDeviceURIs () const; + + std::size_t + getNumOfConnectedDevices () const; + + boost::shared_ptr + getAnyDevice (); + + boost::shared_ptr + getDevice (const std::string& device_URI); + + boost::shared_ptr + getDeviceByIndex (int index); + + boost::shared_ptr + getFileDevice (const std::string& path); + + protected: + boost::shared_ptr device_listener_; + }; + + std::ostream& + operator<< (std::ostream& stream, const OpenNI2DeviceManager& device_manager); + + } // namespace + } +} + +#endif // PCL_IO_OPENNI2_DEVICE_MANAGER_H_ diff --git a/io/include/pcl/io/openni2/openni2_frame_listener.h b/io/include/pcl/io/openni2/openni2_frame_listener.h new file mode 100644 index 00000000000..6dfcb886910 --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_frame_listener.h @@ -0,0 +1,89 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, 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. + */ + +#ifndef PCL_IO_OPENNI2_FRAME_LISTENER_H_ +#define PCL_IO_OPENNI2_FRAME_LISTENER_H_ + +#include + +#include "OpenNI.h" + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + typedef boost::function StreamCallbackFunction; + + /* Each NewFrameListener may only listen to one VideoStream at a time. + **/ + class OpenNI2FrameListener : public openni::VideoStream::NewFrameListener + { + public: + + OpenNI2FrameListener () + : callback_(0) {} + OpenNI2FrameListener (StreamCallbackFunction cb) + : callback_(cb) {} + + virtual ~OpenNI2FrameListener () + { }; + + inline void + onNewFrame (openni::VideoStream& stream) + { + if (callback_) + callback_(stream); + } + + void + setCallback (StreamCallbackFunction cb) + { + callback_ = cb; + } + + private: + StreamCallbackFunction callback_; + }; + + } // namespace + } +} + +#endif // PCL_IO_OPENNI2_FRAME_LISTENER_H_ diff --git a/io/include/pcl/io/openni2/openni2_metadata_wrapper.h b/io/include/pcl/io/openni2/openni2_metadata_wrapper.h new file mode 100644 index 00000000000..4e1cc6c5365 --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_metadata_wrapper.h @@ -0,0 +1,116 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, respective authors. + * + * 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. + * + */ + +#pragma once +#ifndef PCL_IO_OPENNI2_METADATA_WRAPPER_H_ +#define PCL_IO_OPENNI2_METADATA_WRAPPER_H_ + +#include + +#if defined(HAVE_OPENNI2) + +#include +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + class Openni2FrameWrapper : public pcl::io::FrameWrapper + { + public: + Openni2FrameWrapper (openni::VideoFrameRef metadata) + : metadata_(metadata) + {} + + virtual inline const void* + getData () const + { + return (metadata_.getData ()); + } + + virtual inline unsigned + getDataSize () const + { + return (metadata_.getDataSize ()); + } + + virtual inline unsigned + getWidth () const + { + return (metadata_.getWidth ()); + } + + virtual inline unsigned + getHeight () const + { + return (metadata_.getHeight ()); + } + + virtual inline unsigned + getFrameID () const + { + return (metadata_.getFrameIndex ()); + } + + virtual inline uint64_t + getTimestamp () const + { + return (metadata_.getTimestamp ()); + } + + + const inline openni::VideoFrameRef& + getMetaData () const + { + return (metadata_); + } + + private: + openni::VideoFrameRef metadata_; // Internally reference counted + }; + + } // namespace + } +} +#endif // HAVE_OPENNI2 + +#endif // PCL_IO_OPENNI2_METADATA_WRAPPER_H_ diff --git a/io/include/pcl/io/openni2/openni2_timer_filter.h b/io/include/pcl/io/openni2/openni2_timer_filter.h new file mode 100644 index 00000000000..c2d78c0ffa0 --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_timer_filter.h @@ -0,0 +1,74 @@ +/* + * Copyright (c) 2013, Willow Garage, 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 Willow Garage, Inc. 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. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#ifndef PCL_IO_OPENNI2_TIME_FILTER_H_ +#define PCL_IO_OPENNI2_TIME_FILTER_H_ + +#include + +#include "OpenNI.h" + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + class OpenNI2TimerFilter + { + public: + OpenNI2TimerFilter (std::size_t filter_len); + virtual ~OpenNI2TimerFilter (); + + void + addSample (double sample); + + double + getMedian (); + + double + getMovingAvg (); + + void + clear (); + + private: + std::size_t filter_len_; + + std::deque buffer_; + }; + + } // namespace + } +} + +#endif // PCL_IO_OPENNI2_TIME_FILTER_H_ diff --git a/io/include/pcl/io/openni2/openni2_video_mode.h b/io/include/pcl/io/openni2/openni2_video_mode.h new file mode 100644 index 00000000000..7c6fdd8ca10 --- /dev/null +++ b/io/include/pcl/io/openni2/openni2_video_mode.h @@ -0,0 +1,93 @@ +/* + * Copyright (c) 2013, Willow Garage, 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 Willow Garage, Inc. 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. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#ifndef PCL_IO_OPENNI2_VIDEO_MODE_H_ +#define PCL_IO_OPENNI2_VIDEO_MODE_H_ + +#include +#include + +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + // copied from OniEnums.h + typedef enum + { + // Depth + PIXEL_FORMAT_DEPTH_1_MM = 100, + PIXEL_FORMAT_DEPTH_100_UM = 101, + PIXEL_FORMAT_SHIFT_9_2 = 102, + PIXEL_FORMAT_SHIFT_9_3 = 103, + + // Color + PIXEL_FORMAT_RGB888 = 200, + PIXEL_FORMAT_YUV422 = 201, + PIXEL_FORMAT_GRAY8 = 202, + PIXEL_FORMAT_GRAY16 = 203, + PIXEL_FORMAT_JPEG = 204, + PIXEL_FORMAT_YUYV = 205, + } PixelFormat; + + struct OpenNI2VideoMode + { + OpenNI2VideoMode () + :x_resolution_(0), y_resolution_(0), frame_rate_(0) + {} + + OpenNI2VideoMode (int xResolution, int yResolution, int frameRate) + :x_resolution_(xResolution), y_resolution_(yResolution), frame_rate_(frameRate) + {} + + int x_resolution_; + int y_resolution_; + int frame_rate_; + PixelFormat pixel_format_; + }; + + std::ostream& + operator<< (std::ostream& stream, const OpenNI2VideoMode& video_mode); + + bool + operator== (const OpenNI2VideoMode& video_mode_a, const OpenNI2VideoMode& video_mode_b); + + bool + operator!= (const OpenNI2VideoMode& video_mode_a, const OpenNI2VideoMode& video_mode_b); + + } // namespace + } +} + +#endif diff --git a/io/include/pcl/io/openni2/openni_shift_to_depth_conversion.h b/io/include/pcl/io/openni2/openni_shift_to_depth_conversion.h new file mode 100644 index 00000000000..75d429c84a6 --- /dev/null +++ b/io/include/pcl/io/openni2/openni_shift_to_depth_conversion.h @@ -0,0 +1,124 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2011 Willow Garage, Inc. + * Copyright (c) 2012-, 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 +#ifdef HAVE_OPENNI2 + +#ifndef __OPENNI_SHIFT_TO_DEPTH_CONVERSION +#define __OPENNI_SHIFT_TO_DEPTH_CONVERSION + +#include +#include + +namespace openni_wrapper +{ + /** \brief This class provides conversion of the openni 11-bit shift data to depth; + */ + class PCL_EXPORTS ShiftToDepthConverter + { + public: + /** \brief Constructor. */ + ShiftToDepthConverter () : init_(false) {} + + /** \brief Destructor. */ + virtual ~ShiftToDepthConverter () {}; + + /** \brief This method generates a look-up table to convert openni shift values to depth + */ + void + generateLookupTable () + { + // lookup of 11 bit shift values + const std::size_t table_size = 1<<10; + + lookupTable_.clear(); + lookupTable_.resize(table_size); + + // constants taken from openni driver + static const int16_t nConstShift = 800; + static const double nParamCoeff = 4.000000; + static const double dPlanePixelSize = 0.104200; + static const double nShiftScale = 10.000000; + static const double dPlaneDsr = 120.000000; + static const double dPlaneDcl = 7.500000; + + std::size_t i; + double dFixedRefX; + double dMetric; + + for (i=0; i(i - nConstShift) / nParamCoeff)-0.375; + dMetric = dFixedRefX * dPlanePixelSize; + lookupTable_[i] = static_cast((nShiftScale * ((dMetric * dPlaneDsr / (dPlaneDcl - dMetric)) + dPlaneDsr) ) / 1000.0f); + } + + init_ = true; + } + + /** \brief Generate a look-up table for converting openni shift values to depth + */ + inline float + shiftToDepth (uint16_t shift_val) + { + assert (init_); + + static const float bad_point = std::numeric_limits::quiet_NaN (); + + float ret = bad_point; + + // lookup depth value in shift lookup table + if (shift_val lookupTable_; + bool init_; + } ; +} + +#endif +#endif //__OPENNI_SHIFT_TO_DEPTH_CONVERSION diff --git a/io/include/pcl/io/openni2_grabber.h b/io/include/pcl/io/openni2_grabber.h new file mode 100644 index 00000000000..a41fbfaf7ea --- /dev/null +++ b/io/include/pcl/io/openni2_grabber.h @@ -0,0 +1,505 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, respective authors. + * + * 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 +#ifdef HAVE_OPENNI2 + +#ifndef PCL_IO_OPENNI2_GRABBER_H_ +#define PCL_IO_OPENNI2_GRABBER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace pcl +{ + struct PointXYZ; + struct PointXYZRGB; + struct PointXYZRGBA; + struct PointXYZI; + template class PointCloud; + + namespace io + { + + /** \brief Grabber for OpenNI 2 devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live) + * \ingroup io + */ + class PCL_EXPORTS OpenNI2Grabber : public Grabber + { + public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + // Templated images + typedef pcl::io::DepthImage DepthImage; + typedef pcl::io::IRImage IRImage; + typedef pcl::io::Image Image; + + /** \brief Basic camera parameters placeholder. */ + struct CameraParameters + { + /** fx */ + double focal_length_x; + /** fy */ + double focal_length_y; + /** cx */ + double principal_point_x; + /** cy */ + double principal_point_y; + + CameraParameters (double initValue) + : focal_length_x (initValue), focal_length_y (initValue), + principal_point_x (initValue), principal_point_y (initValue) + {} + + CameraParameters (double fx, double fy, double cx, double cy) + : focal_length_x (fx), focal_length_y (fy), principal_point_x (cx), principal_point_y (cy) + { } + }; + + typedef enum + { + OpenNI_Default_Mode = 0, // This can depend on the device. For now all devices (PSDK, Xtion, Kinect) its VGA@30Hz + OpenNI_SXGA_15Hz = 1, // Only supported by the Kinect + OpenNI_VGA_30Hz = 2, // Supported by PSDK, Xtion and Kinect + OpenNI_VGA_25Hz = 3, // Supportged by PSDK and Xtion + OpenNI_QVGA_25Hz = 4, // Supported by PSDK and Xtion + OpenNI_QVGA_30Hz = 5, // Supported by PSDK, Xtion and Kinect + OpenNI_QVGA_60Hz = 6, // Supported by PSDK and Xtion + OpenNI_QQVGA_25Hz = 7, // Not supported -> using software downsampling (only for integer scale factor and only NN) + OpenNI_QQVGA_30Hz = 8, // Not supported -> using software downsampling (only for integer scale factor and only NN) + OpenNI_QQVGA_60Hz = 9 // Not supported -> using software downsampling (only for integer scale factor and only NN) + } Mode; + + //define callback signature typedefs + typedef void (sig_cb_openni_image) (const boost::shared_ptr&); + typedef void (sig_cb_openni_depth_image) (const boost::shared_ptr&); + typedef void (sig_cb_openni_ir_image) (const boost::shared_ptr&); + typedef void (sig_cb_openni_image_depth_image) (const boost::shared_ptr&, const boost::shared_ptr&, float reciprocalFocalLength) ; + typedef void (sig_cb_openni_ir_depth_image) (const boost::shared_ptr&, const boost::shared_ptr&, float reciprocalFocalLength) ; + typedef void (sig_cb_openni_point_cloud) (const boost::shared_ptr >&); + typedef void (sig_cb_openni_point_cloud_rgb) (const boost::shared_ptr >&); + typedef void (sig_cb_openni_point_cloud_rgba) (const boost::shared_ptr >&); + typedef void (sig_cb_openni_point_cloud_i) (const boost::shared_ptr >&); + + public: + /** \brief Constructor + * \param[in] device_id ID of the device, which might be a serial number, bus@address or the index of the device. + * \param[in] depth_mode the mode of the depth stream + * \param[in] image_mode the mode of the image stream + */ + OpenNI2Grabber (const std::string& device_id = "", + const Mode& depth_mode = OpenNI_Default_Mode, + const Mode& image_mode = OpenNI_Default_Mode); + + /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */ + virtual ~OpenNI2Grabber () throw (); + + /** \brief Start the data acquisition. */ + virtual void + start (); + + /** \brief Stop the data acquisition. */ + virtual void + stop (); + + /** \brief Check if the data acquisition is still running. */ + virtual bool + isRunning () const; + + virtual std::string + getName () const; + + /** \brief Obtain the number of frames per second (FPS). */ + virtual float + getFramesPerSecond () const; + + /** \brief Get a boost shared pointer to the \ref OpenNIDevice object. */ + inline boost::shared_ptr + getDevice () const; + + /** \brief Obtain a list of the available depth modes that this device supports. */ + std::vector > + getAvailableDepthModes () const; + + /** \brief Obtain a list of the available image modes that this device supports. */ + std::vector > + getAvailableImageModes () const; + + /** \brief Set the RGB camera parameters (fx, fy, cx, cy) + * \param[in] rgb_focal_length_x the RGB focal length (fx) + * \param[in] rgb_focal_length_y the RGB focal length (fy) + * \param[in] rgb_principal_point_x the RGB principal point (cx) + * \param[in] rgb_principal_point_y the RGB principal point (cy) + * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + */ + inline void + setRGBCameraIntrinsics (const double rgb_focal_length_x, + const double rgb_focal_length_y, + const double rgb_principal_point_x, + const double rgb_principal_point_y) + { + rgb_parameters_ = CameraParameters ( + rgb_focal_length_x, rgb_focal_length_y, + rgb_principal_point_x, rgb_principal_point_y); + } + + /** \brief Get the RGB camera parameters (fx, fy, cx, cy) + * \param[out] rgb_focal_length_x the RGB focal length (fx) + * \param[out] rgb_focal_length_y the RGB focal length (fy) + * \param[out] rgb_principal_point_x the RGB principal point (cx) + * \param[out] rgb_principal_point_y the RGB principal point (cy) + */ + inline void + getRGBCameraIntrinsics (double &rgb_focal_length_x, + double &rgb_focal_length_y, + double &rgb_principal_point_x, + double &rgb_principal_point_y) const + { + rgb_focal_length_x = rgb_parameters_.focal_length_x; + rgb_focal_length_y = rgb_parameters_.focal_length_y; + rgb_principal_point_x = rgb_parameters_.principal_point_x; + rgb_principal_point_y = rgb_parameters_.principal_point_y; + } + + + /** \brief Set the RGB image focal length (fx = fy). + * \param[in] rgb_focal_length the RGB focal length (assumes fx = fy) + * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it + * and the grabber will use the default values from the camera instead. + * These parameters will be used for XYZRGBA clouds. + */ + inline void + setRGBFocalLength (const double rgb_focal_length) + { + rgb_parameters_.focal_length_x = rgb_focal_length; + rgb_parameters_.focal_length_y = rgb_focal_length; + } + + /** \brief Set the RGB image focal length + * \param[in] rgb_focal_length_x the RGB focal length (fx) + * \param[in] rgb_focal_ulength_y the RGB focal length (fy) + * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + * These parameters will be used for XYZRGBA clouds. + */ + inline void + setRGBFocalLength (const double rgb_focal_length_x, const double rgb_focal_length_y) + { + rgb_parameters_.focal_length_x = rgb_focal_length_x; + rgb_parameters_.focal_length_y = rgb_focal_length_y; + } + + /** \brief Return the RGB focal length parameters (fx, fy) + * \param[out] rgb_focal_length_x the RGB focal length (fx) + * \param[out] rgb_focal_length_y the RGB focal length (fy) + */ + inline void + getRGBFocalLength (double &rgb_focal_length_x, double &rgb_focal_length_y) const + { + rgb_focal_length_x = rgb_parameters_.focal_length_x; + rgb_focal_length_y = rgb_parameters_.focal_length_y; + } + + /** \brief Set the Depth camera parameters (fx, fy, cx, cy) + * \param[in] depth_focal_length_x the Depth focal length (fx) + * \param[in] depth_focal_length_y the Depth focal length (fy) + * \param[in] depth_principal_point_x the Depth principal point (cx) + * \param[in] depth_principal_point_y the Depth principal point (cy) + * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + */ + inline void + setDepthCameraIntrinsics (const double depth_focal_length_x, + const double depth_focal_length_y, + const double depth_principal_point_x, + const double depth_principal_point_y) + { + depth_parameters_ = CameraParameters ( + depth_focal_length_x, depth_focal_length_y, + depth_principal_point_x, depth_principal_point_y); + } + + /** \brief Get the Depth camera parameters (fx, fy, cx, cy) + * \param[out] depth_focal_length_x the Depth focal length (fx) + * \param[out] depth_focal_length_y the Depth focal length (fy) + * \param[out] depth_principal_point_x the Depth principal point (cx) + * \param[out] depth_principal_point_y the Depth principal point (cy) + */ + inline void + getDepthCameraIntrinsics (double &depth_focal_length_x, + double &depth_focal_length_y, + double &depth_principal_point_x, + double &depth_principal_point_y) const + { + depth_focal_length_x = depth_parameters_.focal_length_x; + depth_focal_length_y = depth_parameters_.focal_length_y; + depth_principal_point_x = depth_parameters_.principal_point_x; + depth_principal_point_y = depth_parameters_.principal_point_y; + } + + /** \brief Set the Depth image focal length (fx = fy). + * \param[in] depth_focal_length the Depth focal length (assumes fx = fy) + * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it + * and the grabber will use the default values from the camera instead. + */ + inline void + setDepthFocalLength (const double depth_focal_length) + { + depth_parameters_.focal_length_x = depth_focal_length; + depth_parameters_.focal_length_y = depth_focal_length; + } + + + /** \brief Set the Depth image focal length + * \param[in] depth_focal_length_x the Depth focal length (fx) + * \param[in] depth_focal_length_y the Depth focal length (fy) + * Setting the parameter to non-finite values (e.g., NaN, Inf) invalidates them + * and the grabber will use the default values from the camera instead. + */ + inline void + setDepthFocalLength (const double depth_focal_length_x, const double depth_focal_length_y) + { + depth_parameters_.focal_length_x = depth_focal_length_x; + depth_parameters_.focal_length_y = depth_focal_length_y; + } + + /** \brief Return the Depth focal length parameters (fx, fy) + * \param[out] depth_focal_length_x the Depth focal length (fx) + * \param[out] depth_focal_length_y the Depth focal length (fy) + */ + inline void + getDepthFocalLength (double &depth_focal_length_x, double &depth_focal_length_y) const + { + depth_focal_length_x = depth_parameters_.focal_length_x; + depth_focal_length_y = depth_parameters_.focal_length_y; + } + + protected: + + /** \brief Sets up an OpenNI device. */ + void + setupDevice (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode); + + /** \brief Update mode maps. */ + void + updateModeMaps (); + + /** \brief Start synchronization. */ + void + startSynchronization (); + + /** \brief Stop synchronization. */ + void + stopSynchronization (); + + // TODO: rename to mapMode2OniMode + /** \brief Map config modes. */ + bool + mapMode2XnMode (int mode, pcl::io::openni2::OpenNI2VideoMode& videoMode) const; + + // callback methods + /** \brief RGB image callback. */ + virtual void + imageCallback (pcl::io::openni2::Image::Ptr image, void* cookie); + + /** \brief Depth image callback. */ + virtual void + depthCallback (pcl::io::openni2::DepthImage::Ptr depth_image, void* cookie); + + /** \brief IR image callback. */ + virtual void + irCallback (pcl::io::openni2::IRImage::Ptr ir_image, void* cookie); + + /** \brief RGB + Depth image callback. */ + virtual void + imageDepthImageCallback (const pcl::io::openni2::Image::Ptr &image, + const pcl::io::openni2::DepthImage::Ptr &depth_image); + + /** \brief IR + Depth image callback. */ + virtual void + irDepthImageCallback (const pcl::io::openni2::IRImage::Ptr &image, + const pcl::io::openni2::DepthImage::Ptr &depth_image); + + /** \brief Process changed signals. */ + virtual void + signalsChanged (); + + // helper methods + + /** \brief Check if the RGB and Depth images are required to be synchronized or not. */ + virtual void + checkImageAndDepthSynchronizationRequired (); + + /** \brief Check if the RGB image stream is required or not. */ + virtual void + checkImageStreamRequired (); + + /** \brief Check if the depth stream is required or not. */ + virtual void + checkDepthStreamRequired (); + + /** \brief Check if the IR image stream is required or not. */ + virtual void + checkIRStreamRequired (); + + + // Point cloud conversion /////////////////////////////////////////////// + + /** \brief Convert a Depth image to a pcl::PointCloud + * \param[in] depth the depth image to convert + */ + boost::shared_ptr > + convertToXYZPointCloud (const pcl::io::openni2::DepthImage::Ptr &depth) const; + + /** \brief Convert a Depth + RGB image pair to a pcl::PointCloud + * \param[in] image the RGB image to convert + * \param[in] depth_image the depth image to convert + */ + template typename pcl::PointCloud::Ptr + convertToXYZRGBPointCloud (const pcl::io::openni2::Image::Ptr &image, + const pcl::io::openni2::DepthImage::Ptr &depth_image) const; + + /** \brief Convert a Depth + Intensity image pair to a pcl::PointCloud + * \param[in] image the IR image to convert + * \param[in] depth_image the depth image to convert + */ + boost::shared_ptr > + convertToXYZIPointCloud (const pcl::io::openni2::IRImage::Ptr &image, + const pcl::io::openni2::DepthImage::Ptr &depth_image) const; + + + // Stream callbacks ///////////////////////////////////////////////////// + void + processColorFrame (openni::VideoStream& stream); + + void + processDepthFrame (openni::VideoStream& stream); + + void + processIRFrame (openni::VideoStream& stream); + + + Synchronizer rgb_sync_; + Synchronizer ir_sync_; + + /** \brief The actual openni device. */ + boost::shared_ptr device_; + + std::string rgb_frame_id_; + std::string depth_frame_id_; + unsigned image_width_; + unsigned image_height_; + unsigned depth_width_; + unsigned depth_height_; + + bool image_required_; + bool depth_required_; + bool ir_required_; + bool sync_required_; + + boost::signals2::signal* image_signal_; + boost::signals2::signal* depth_image_signal_; + boost::signals2::signal* ir_image_signal_; + boost::signals2::signal* image_depth_image_signal_; + boost::signals2::signal* ir_depth_image_signal_; + boost::signals2::signal* point_cloud_signal_; + boost::signals2::signal* point_cloud_i_signal_; + boost::signals2::signal* point_cloud_rgb_signal_; + boost::signals2::signal* point_cloud_rgba_signal_; + + struct modeComp + { + bool operator () (const openni::VideoMode& mode1, const openni::VideoMode & mode2) const + { + if (mode1.getResolutionX () < mode2.getResolutionX ()) + return true; + else if (mode1.getResolutionX () > mode2.getResolutionX ()) + return false; + else if (mode1.getResolutionY () < mode2.getResolutionY ()) + return true; + else if (mode1.getResolutionY () > mode2.getResolutionY ()) + return false; + else if (mode1.getFps () < mode2.getFps () ) + return true; + else + return false; + } + }; + + // Mapping from config (enum) modes to native OpenNI modes + std::map config2oni_map_; + + pcl::io::openni2::OpenNI2Device::CallbackHandle depth_callback_handle_; + pcl::io::openni2::OpenNI2Device::CallbackHandle image_callback_handle_; + pcl::io::openni2::OpenNI2Device::CallbackHandle ir_callback_handle_; + bool running_; + + + CameraParameters rgb_parameters_; + CameraParameters depth_parameters_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; + + boost::shared_ptr + OpenNI2Grabber::getDevice () const + { + return device_; + } + + } // namespace +} + +#endif // PCL_IO_OPENNI2_GRABBER_H_ +#endif // HAVE_OPENNI2 diff --git a/io/src/openni2/openni2_convert.cpp b/io/src/openni2/openni2_convert.cpp new file mode 100644 index 00000000000..39f1e712ed5 --- /dev/null +++ b/io/src/openni2/openni2_convert.cpp @@ -0,0 +1,107 @@ +/* + * Copyright (c) 2013, Willow Garage, 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 Willow Garage, Inc. 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. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#include "pcl/io/openni2/openni2_convert.h" +#include "pcl/io/io_exception.h" + +#include + +#include + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + const OpenNI2DeviceInfo + openni2_convert (const openni::DeviceInfo* pInfo) + { + if (!pInfo) + THROW_IO_EXCEPTION ("openni2_convert called with zero pointer\n"); + + OpenNI2DeviceInfo output; + + output.name_ = pInfo->getName (); + output.uri_ = pInfo->getUri (); + output.vendor_ = pInfo->getVendor (); + output.product_id_ = pInfo->getUsbProductId (); + output.vendor_id_ = pInfo->getUsbVendorId (); + + return (output); + } + + const openni::VideoMode + grabberModeToOpenniMode (const OpenNI2VideoMode& input) + { + + openni::VideoMode output; + + output.setResolution (input.x_resolution_, input.y_resolution_); + output.setFps (input.frame_rate_); + output.setPixelFormat (static_cast(input.pixel_format_)); + + return (output); + } + + + const OpenNI2VideoMode + openniModeToGrabberMode (const openni::VideoMode& input) + { + OpenNI2VideoMode output; + + output.x_resolution_ = input.getResolutionX (); + output.y_resolution_ = input.getResolutionY (); + output.frame_rate_ = input.getFps (); + output.pixel_format_ = static_cast(input.getPixelFormat ()); + + return (output); + } + + const std::vector + openniModeToGrabberMode (const openni::Array& input) + { + std::vector output; + + int size = input.getSize (); + + output.reserve (size); + + for (int i=0; i +#include // For XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE property + +#include +#include +#include +#include +#include + +#include "pcl/io/openni2/openni2_device.h" +#include "pcl/io/openni2/openni2_convert.h" +#include "pcl/io/openni2/openni2_frame_listener.h" + +#include "pcl/io/io_exception.h" + +#include + +using namespace openni; +using namespace pcl::io::openni2; + +using openni::VideoMode; +using std::vector; + +typedef boost::chrono::high_resolution_clock hr_clock; + +pcl::io::openni2::OpenNI2Device::OpenNI2Device (const std::string& device_URI) : + openni_device_(), + ir_video_started_(false), + color_video_started_(false), + depth_video_started_(false) +{ + openni::Status status = openni::OpenNI::initialize (); + if (status != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Initialize failed\n%s\n", OpenNI::getExtendedError ()); + + openni_device_ = boost::make_shared(); + + if (device_URI.length () > 0) + status = openni_device_->open (device_URI.c_str ()); + else + status = openni_device_->open (openni::ANY_DEVICE); + + if (status != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Initialize failed\n%s\n", openni::OpenNI::getExtendedError ()); + + // Get depth calculation parameters + // Some of these are device-spefic and may not exist + baseline_ = 0.0; + if ( getDepthVideoStream ()->isPropertySupported (XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE) ) + { + double baseline; + getDepthVideoStream ()->getProperty (XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE, &baseline); // Device specific -- from PS1080.h + baseline_ = static_cast (baseline * 0.01f); // baseline from cm -> meters + } + shadow_value_ = 0; // This does not exist in OpenNI 2, and is maintained for compatibility with the OpenNI 1.x grabber + no_sample_value_ = 0; // This does not exist in OpenNI 2, and is maintained for compatibility with the OpenNI 1.x grabber + + // Set default resolution if not reading a file + if (!openni_device_->isFile ()) + { + setColorVideoMode (getDefaultColorMode ()); + setDepthVideoMode (getDefaultDepthMode ()); + setIRVideoMode (getDefaultIRMode ()); + } + + if (openni_device_->isFile ()) + { + openni_device_->getPlaybackControl ()->setSpeed (1.0f); + } + + device_info_ = boost::make_shared(); + *device_info_ = openni_device_->getDeviceInfo (); + + color_frame_listener = boost::make_shared(); + depth_frame_listener = boost::make_shared(); + ir_frame_listener = boost::make_shared(); +} + +pcl::io::openni2::OpenNI2Device::~OpenNI2Device () +{ + stopAllStreams (); + + shutdown (); + + openni_device_->close (); +} + +const std::string +pcl::io::openni2::OpenNI2Device::getUri () const +{ + return (std::string (device_info_->getUri ())); +} + +const std::string +pcl::io::openni2::OpenNI2Device::getVendor () const +{ + return (std::string (device_info_->getVendor ())); +} + +const std::string +pcl::io::openni2::OpenNI2Device::getName () const +{ + return (std::string (device_info_->getName ())); +} + +uint16_t +pcl::io::openni2::OpenNI2Device::getUsbVendorId () const +{ + return (device_info_->getUsbVendorId ()); +} + +uint16_t +pcl::io::openni2::OpenNI2Device::getUsbProductId () const +{ + return (device_info_->getUsbProductId ()); +} + +const std::string +pcl::io::openni2::OpenNI2Device::getStringID () const +{ + std::string ID_str = getName () + "_" + getVendor (); + + boost::replace_all (ID_str, "/", ""); + boost::replace_all (ID_str, ".", ""); + boost::replace_all (ID_str, "@", ""); + + return (ID_str); +} + +bool +pcl::io::openni2::OpenNI2Device::isValid () const +{ + return (openni_device_.get () != 0) && openni_device_->isValid (); +} + +float +pcl::io::openni2::OpenNI2Device::getIRFocalLength (int output_x_resolution) const +{ + boost::shared_ptr stream = getIRVideoStream (); + + int frameWidth = stream->getVideoMode ().getResolutionX (); + float hFov = stream->getHorizontalFieldOfView (); + float calculatedFocalLengthX = frameWidth / (2.0f * tan (hFov / 2.0f)); + return (calculatedFocalLengthX); +} + +float +pcl::io::openni2::OpenNI2Device::getColorFocalLength (int output_x_resolution) const +{ + boost::shared_ptr stream = getColorVideoStream (); + + int frameWidth = stream->getVideoMode ().getResolutionX (); + float hFov = stream->getHorizontalFieldOfView (); + float calculatedFocalLengthX = frameWidth / (2.0f * tan (hFov / 2.0f)); + return (calculatedFocalLengthX); +} + +float +pcl::io::openni2::OpenNI2Device::getDepthFocalLength (int output_x_resolution) const +{ + boost::shared_ptr stream = getDepthVideoStream (); + + int frameWidth = stream->getVideoMode ().getResolutionX (); + float hFov = stream->getHorizontalFieldOfView (); + float calculatedFocalLengthX = frameWidth / (2.0f * tan (hFov / 2.0f)); + return (calculatedFocalLengthX); +} + +float +pcl::io::openni2::OpenNI2Device::getBaseline() +{ + return (baseline_); +} + +uint64_t +pcl::io::openni2::OpenNI2Device::getShadowValue() +{ + return (shadow_value_); +} + +bool +pcl::io::openni2::OpenNI2Device::isIRVideoModeSupported (const OpenNI2VideoMode& video_mode) const +{ + getSupportedIRVideoModes (); + + bool supported = false; + + std::vector::const_iterator it = ir_video_modes_.begin (); + std::vector::const_iterator it_end = ir_video_modes_.end (); + + while (it != it_end && !supported) + { + supported = (*it == video_mode); + ++it; + } + + return (supported); +} + +bool +pcl::io::openni2::OpenNI2Device::isColorVideoModeSupported (const OpenNI2VideoMode& video_mode) const +{ + getSupportedColorVideoModes (); + + bool supported = false; + + std::vector::const_iterator it = color_video_modes_.begin (); + std::vector::const_iterator it_end = color_video_modes_.end (); + + while (it != it_end && !supported) + { + supported = (*it == video_mode); + ++it; + } + + return (supported); +} + +bool +pcl::io::openni2::OpenNI2Device::isDepthVideoModeSupported (const OpenNI2VideoMode& video_mode) const +{ + getSupportedDepthVideoModes (); + + bool supported = false; + + std::vector::const_iterator it = depth_video_modes_.begin (); + std::vector::const_iterator it_end = depth_video_modes_.end (); + + while (it != it_end && !supported) + { + supported = (*it == video_mode); + ++it; + } + + return (supported); +} + +bool +pcl::io::openni2::OpenNI2Device::hasIRSensor () const +{ + return (openni_device_->hasSensor (openni::SENSOR_IR)); +} + +bool +pcl::io::openni2::OpenNI2Device::hasColorSensor () const +{ + return (openni_device_->hasSensor (openni::SENSOR_COLOR)); +} + +bool +pcl::io::openni2::OpenNI2Device::hasDepthSensor () const +{ + return (openni_device_->hasSensor (openni::SENSOR_DEPTH)); +} + +void +pcl::io::openni2::OpenNI2Device::startIRStream () +{ + boost::shared_ptr stream = getIRVideoStream (); + + if (stream) + { + stream->setMirroringEnabled (false); + stream->addNewFrameListener (ir_frame_listener.get ()); + stream->start (); + ir_video_started_ = true; + } +} + +void +pcl::io::openni2::OpenNI2Device::startColorStream () +{ + boost::shared_ptr stream = getColorVideoStream (); + + if (stream) + { + stream->setMirroringEnabled (false); + stream->addNewFrameListener (color_frame_listener.get ()); + stream->start (); + color_video_started_ = true; + } +} +void +pcl::io::openni2::OpenNI2Device::startDepthStream () +{ + boost::shared_ptr stream = getDepthVideoStream (); + + if (stream) + { + stream->setMirroringEnabled (false); + stream->addNewFrameListener (depth_frame_listener.get ()); + stream->start (); + depth_video_started_ = true; + } +} + +void +pcl::io::openni2::OpenNI2Device::stopAllStreams () +{ + stopIRStream (); + stopColorStream (); + stopDepthStream (); +} + +void +pcl::io::openni2::OpenNI2Device::stopIRStream () +{ + if (ir_video_stream_.get () != 0) + { + ir_video_stream_->stop (); + ir_video_started_ = false; + } +} +void +pcl::io::openni2::OpenNI2Device::stopColorStream () +{ + if (color_video_stream_.get () != 0) + { + color_video_stream_->stop (); + color_video_started_ = false; + } +} +void +pcl::io::openni2::OpenNI2Device::stopDepthStream () +{ + if (depth_video_stream_.get () != 0) + { + depth_video_stream_->stop (); + depth_video_started_ = false; + } +} + +void +pcl::io::openni2::OpenNI2Device::shutdown () +{ + if (ir_video_stream_.get () != 0) + ir_video_stream_->destroy (); + + if (color_video_stream_.get () != 0) + color_video_stream_->destroy (); + + if (depth_video_stream_.get () != 0) + depth_video_stream_->destroy (); + +} + +bool +pcl::io::openni2::OpenNI2Device::isIRStreamStarted () +{ + return (ir_video_started_); +} +bool +pcl::io::openni2::OpenNI2Device::isColorStreamStarted () +{ + return (color_video_started_); +} +bool +pcl::io::openni2::OpenNI2Device::isDepthStreamStarted () +{ + return (depth_video_started_); +} + +bool +pcl::io::openni2::OpenNI2Device::isImageRegistrationModeSupported () const +{ + return (openni_device_->isImageRegistrationModeSupported (openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR)); +} + +void +pcl::io::openni2::OpenNI2Device::setImageRegistrationMode (bool setEnable) +{ + bool registrationSupported = isImageRegistrationModeSupported (); + if (registrationSupported) + { + if (setEnable) + { + openni::Status rc = openni_device_->setImageRegistrationMode (openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Enabling image registration mode failed: \n%s\n", openni::OpenNI::getExtendedError ()); + } + else + { + openni::Status rc = openni_device_->setImageRegistrationMode (openni::IMAGE_REGISTRATION_OFF); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Disabling image registration mode failed: \n%s\n", openni::OpenNI::getExtendedError ()); + } + } +} + +bool +pcl::io::openni2::OpenNI2Device::isDepthRegistered () const +{ + return openni_device_->getImageRegistrationMode () == openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR; +} + +void +pcl::io::openni2::OpenNI2Device::setSynchronization (bool enabled) +{ + openni::Status rc = openni_device_->setDepthColorSyncEnabled (enabled); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Enabling depth color synchronization failed: \n%s\n", openni::OpenNI::getExtendedError ()); +} + +const OpenNI2VideoMode +pcl::io::openni2::OpenNI2Device::getIRVideoMode () +{ + OpenNI2VideoMode ret; + + boost::shared_ptr stream = getIRVideoStream (); + + if (stream) + { + openni::VideoMode video_mode = stream->getVideoMode (); + + ret = openniModeToGrabberMode (video_mode); + } + else + THROW_IO_EXCEPTION ("Could not create video stream."); + + return (ret); +} + +const OpenNI2VideoMode +pcl::io::openni2::OpenNI2Device::getColorVideoMode () +{ + OpenNI2VideoMode ret; + + boost::shared_ptr stream = getColorVideoStream (); + + if (stream) + { + openni::VideoMode video_mode = stream->getVideoMode (); + + ret = openniModeToGrabberMode (video_mode); + } + else + THROW_IO_EXCEPTION ("Could not create video stream."); + + return (ret); +} + +const OpenNI2VideoMode +pcl::io::openni2::OpenNI2Device::getDepthVideoMode () +{ + OpenNI2VideoMode ret; + + boost::shared_ptr stream = getDepthVideoStream (); + + if (stream) + { + openni::VideoMode video_mode = stream->getVideoMode (); + + ret = openniModeToGrabberMode (video_mode); + } + else + THROW_IO_EXCEPTION ("Could not create video stream."); + + return (ret); +} + +void +pcl::io::openni2::OpenNI2Device::setIRVideoMode (const OpenNI2VideoMode& video_mode) +{ + boost::shared_ptr stream = getIRVideoStream (); + + if (stream) + { + const openni::VideoMode videoMode = grabberModeToOpenniMode (video_mode); + const openni::Status rc = stream->setVideoMode (videoMode); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't set IR video mode: \n%s\n", openni::OpenNI::getExtendedError ()); + } +} + +void +pcl::io::openni2::OpenNI2Device::setColorVideoMode (const OpenNI2VideoMode& video_mode) +{ + boost::shared_ptr stream = getColorVideoStream (); + + if (stream) + { + openni::VideoMode videoMode = grabberModeToOpenniMode (video_mode); + const openni::Status rc = stream->setVideoMode (videoMode); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't set color video mode: \n%s\n", openni::OpenNI::getExtendedError ()); + } +} + +void +pcl::io::openni2::OpenNI2Device::setDepthVideoMode (const OpenNI2VideoMode& video_mode) +{ + boost::shared_ptr stream = getDepthVideoStream (); + + if (stream) + { + const openni::VideoMode videoMode = grabberModeToOpenniMode (video_mode); + const openni::Status rc = stream->setVideoMode (videoMode); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't set depth video mode: \n%s\n", openni::OpenNI::getExtendedError ()); + } +} + +OpenNI2VideoMode +pcl::io::openni2::OpenNI2Device::getDefaultIRMode () const +{ + // Search for and return VGA@30 Hz mode + vector modeList = getSupportedIRVideoModes (); + for (vector::iterator modeItr = modeList.begin (); modeItr != modeList.end (); modeItr++) + { + OpenNI2VideoMode mode = *modeItr; + if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ = 30.0) ) + return mode; + } + return (modeList.at (0)); // Return first mode if we can't find VGA +} + +OpenNI2VideoMode +pcl::io::openni2::OpenNI2Device::getDefaultColorMode () const +{ + // Search for and return VGA@30 Hz mode + vector modeList = getSupportedColorVideoModes (); + for (vector::iterator modeItr = modeList.begin (); modeItr != modeList.end (); modeItr++) + { + OpenNI2VideoMode mode = *modeItr; + if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ = 30.0) ) + return mode; + } + return (modeList.at (0)); // Return first mode if we can't find VGA +} + +OpenNI2VideoMode +pcl::io::openni2::OpenNI2Device::getDefaultDepthMode () const +{ + // Search for and return VGA@30 Hz mode + vector modeList = getSupportedDepthVideoModes (); + for (vector::iterator modeItr = modeList.begin (); modeItr != modeList.end (); modeItr++) + { + OpenNI2VideoMode mode = *modeItr; + if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ = 30.0) ) + return mode; + } + return (modeList.at (0)); // Return first mode if we can't find VGA +} + +const std::vector& +pcl::io::openni2::OpenNI2Device::getSupportedIRVideoModes () const +{ + boost::shared_ptr stream = getIRVideoStream (); + ir_video_modes_.clear (); + + if (stream) + { + const openni::SensorInfo& sensor_info = stream->getSensorInfo (); + + ir_video_modes_ = openniModeToGrabberMode (sensor_info.getSupportedVideoModes ()); + } + + return (ir_video_modes_); +} + +const std::vector& +pcl::io::openni2::OpenNI2Device::getSupportedColorVideoModes () const +{ + boost::shared_ptr stream = getColorVideoStream (); + + color_video_modes_.clear (); + + if (stream) + { + const openni::SensorInfo& sensor_info = stream->getSensorInfo (); + + color_video_modes_ = openniModeToGrabberMode (sensor_info.getSupportedVideoModes ()); + } + + return (color_video_modes_); +} + +const std::vector& +pcl::io::openni2::OpenNI2Device::getSupportedDepthVideoModes () const +{ + boost::shared_ptr stream = getDepthVideoStream (); + + depth_video_modes_.clear (); + + if (stream) + { + const openni::SensorInfo& sensor_info = stream->getSensorInfo (); + + depth_video_modes_ = openniModeToGrabberMode (sensor_info.getSupportedVideoModes ()); + } + + return (depth_video_modes_); +} + +bool +pcl::io::openni2::OpenNI2Device::findCompatibleIRMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const +{ + if ( isIRVideoModeSupported (requested_mode) ) + { + actual_mode = requested_mode; + return (true); + } + else + { + // Find a resize-compatable mode + std::vector supportedModes = getSupportedIRVideoModes (); + bool found = findCompatibleVideoMode (supportedModes, requested_mode, actual_mode); + return (found); + } +} + +bool +pcl::io::openni2::OpenNI2Device::findCompatibleColorMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const +{ + if ( isColorVideoModeSupported (requested_mode) ) + { + actual_mode = requested_mode; + return (true); + } + else + { + // Find a resize-compatable mode + std::vector supportedModes = getSupportedColorVideoModes (); + bool found = findCompatibleVideoMode (supportedModes, requested_mode, actual_mode); + return (found); + } +} + +bool +pcl::io::openni2::OpenNI2Device::findCompatibleDepthMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const +{ + if ( isDepthVideoModeSupported (requested_mode) ) + { + actual_mode = requested_mode; + return (true); + } + else + { + // Find a resize-compatable mode + std::vector supportedModes = getSupportedDepthVideoModes (); + bool found = findCompatibleVideoMode (supportedModes, requested_mode, actual_mode); + return (found); + } +} + +// Generic support method for the above findCompatable...Mode calls above +bool +pcl::io::openni2::OpenNI2Device::findCompatibleVideoMode (const std::vector supportedModes, const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const +{ + bool found = false; + for (std::vector::const_iterator modeIt = supportedModes.begin (); modeIt != supportedModes.end (); ++modeIt) + { + if (modeIt->frame_rate_ == requested_mode.frame_rate_ + && resizingSupported (modeIt->x_resolution_, modeIt->y_resolution_, requested_mode.x_resolution_, requested_mode.y_resolution_)) + { + if (found) + { // check wheter the new mode is better -> smaller than the current one. + if (actual_mode.x_resolution_ * actual_mode.x_resolution_ > modeIt->x_resolution_ * modeIt->y_resolution_ ) + actual_mode = *modeIt; + } + else + { + actual_mode = *modeIt; + found = true; + } + } + } + return (found); +} + +bool +pcl::io::openni2::OpenNI2Device::resizingSupported (size_t input_width, size_t input_height, size_t output_width, size_t output_height) const +{ + return (output_width <= input_width && output_height <= input_height && input_width % output_width == 0 && input_height % output_height == 0 ); +} + +void +pcl::io::openni2::OpenNI2Device::setAutoExposure (bool enable) +{ + boost::shared_ptr stream = getColorVideoStream (); + + if (stream) + { + openni::CameraSettings* camera_seeting = stream->getCameraSettings (); + if (camera_seeting) + { + const openni::Status rc = camera_seeting->setAutoExposureEnabled (enable); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't set auto exposure: \n%s\n", openni::OpenNI::getExtendedError ()); + } + + } +} + +void +pcl::io::openni2::OpenNI2Device::setAutoWhiteBalance (bool enable) +{ + boost::shared_ptr stream = getColorVideoStream (); + + if (stream) + { + openni::CameraSettings* camera_seeting = stream->getCameraSettings (); + if (camera_seeting) + { + const openni::Status rc = camera_seeting->setAutoWhiteBalanceEnabled (enable); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't set auto white balance: \n%s\n", openni::OpenNI::getExtendedError ()); + } + + } +} + +bool +pcl::io::openni2::OpenNI2Device::getAutoExposure () const +{ + bool ret = false; + + boost::shared_ptr stream = getColorVideoStream (); + + if (stream) + { + openni::CameraSettings* camera_seeting = stream->getCameraSettings (); + if (camera_seeting) + ret = camera_seeting->getAutoExposureEnabled (); + } + + return (ret); +} + +bool +pcl::io::openni2::OpenNI2Device::getAutoWhiteBalance () const +{ + bool ret = false; + + boost::shared_ptr stream = getColorVideoStream (); + + if (stream) + { + openni::CameraSettings* camera_setting = stream->getCameraSettings (); + if (camera_setting) + ret = camera_setting->getAutoWhiteBalanceEnabled (); + } + + return (ret); +} + +boost::shared_ptr +pcl::io::openni2::OpenNI2Device::getIRVideoStream () const +{ + if (ir_video_stream_.get () == 0) + { + if (hasIRSensor ()) + { + ir_video_stream_ = boost::make_shared(); + + const openni::Status rc = ir_video_stream_->create (*openni_device_, openni::SENSOR_IR); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't create IR video stream: \n%s\n", openni::OpenNI::getExtendedError ()); + } + } + return (ir_video_stream_); +} + +boost::shared_ptr +pcl::io::openni2::OpenNI2Device::getColorVideoStream () const +{ + if (color_video_stream_.get () == 0) + { + if (hasColorSensor ()) + { + color_video_stream_ = boost::make_shared(); + + const openni::Status rc = color_video_stream_->create (*openni_device_, openni::SENSOR_COLOR); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't create color video stream: \n%s\n", openni::OpenNI::getExtendedError ()); + } + } + return (color_video_stream_); +} + +boost::shared_ptr +pcl::io::openni2::OpenNI2Device::getDepthVideoStream () const +{ + if (depth_video_stream_.get () == 0) + { + if (hasDepthSensor ()) + { + depth_video_stream_ = boost::make_shared(); + + const openni::Status rc = depth_video_stream_->create (*openni_device_, openni::SENSOR_DEPTH); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Couldn't create depth video stream: \n%s\n", openni::OpenNI::getExtendedError ()); + } + } + return (depth_video_stream_); +} + +std::ostream& pcl::io::openni2::operator<< (std::ostream& stream, const OpenNI2Device& device) +{ + + stream << "Device info (" << device.getUri () << ")" << std::endl; + stream << " Vendor: " << device.getVendor () << std::endl; + stream << " Name: " << device.getName () << std::endl; + stream << " USB Vendor ID: " << device.getUsbVendorId () << std::endl; + stream << " USB Product ID: " << device.getUsbVendorId () << std::endl << std::endl; + + if (device.hasIRSensor ()) + { + stream << "IR sensor video modes:" << std::endl; + const std::vector& video_modes = device.getSupportedIRVideoModes (); + + std::vector::const_iterator it = video_modes.begin (); + std::vector::const_iterator it_end = video_modes.end (); + for (; it != it_end; ++it) + stream << " - " << *it << std::endl; + } + else + { + stream << "No IR sensor available" << std::endl; + } + + if (device.hasColorSensor ()) + { + stream << "Color sensor video modes:" << std::endl; + const std::vector& video_modes = device.getSupportedColorVideoModes (); + + std::vector::const_iterator it = video_modes.begin (); + std::vector::const_iterator it_end = video_modes.end (); + for (; it != it_end; ++it) + stream << " - " << *it << std::endl; + } + else + { + stream << "No Color sensor available" << std::endl; + } + + if (device.hasDepthSensor ()) + { + stream << "Depth sensor video modes:" << std::endl; + const std::vector& video_modes = device.getSupportedDepthVideoModes (); + + std::vector::const_iterator it = video_modes.begin (); + std::vector::const_iterator it_end = video_modes.end (); + for (; it != it_end; ++it) + stream << " - " << *it << std::endl; + } + else + { + stream << "No Depth sensor available" << std::endl; + } + + return (stream); + +} + +void +pcl::io::openni2::OpenNI2Device::setColorCallback (StreamCallbackFunction color_callback) +{ + color_frame_listener->setCallback (color_callback); +} + +void +pcl::io::openni2::OpenNI2Device::setDepthCallback (StreamCallbackFunction depth_callback) +{ + depth_frame_listener->setCallback (depth_callback); +} + +void +pcl::io::openni2::OpenNI2Device::setIRCallback (StreamCallbackFunction ir_callback) +{ + ir_frame_listener->setCallback (ir_callback); +} diff --git a/io/src/openni2/openni2_device_info.cpp b/io/src/openni2/openni2_device_info.cpp new file mode 100644 index 00000000000..d3f9fadc1d5 --- /dev/null +++ b/io/src/openni2/openni2_device_info.cpp @@ -0,0 +1,54 @@ +/* + * Copyright (c) 2013, Willow Garage, 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 Willow Garage, Inc. 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. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#include +#include "pcl/io/openni2/openni2_device_info.h" + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + std::ostream& operator << (std::ostream& stream, const OpenNI2DeviceInfo& device_info) + { + stream << "Uri: " << device_info.uri_ << " (Vendor: " << device_info.vendor_ << + ", Name: " << device_info.name_ << + ", Vendor ID: " << device_info.vendor_id_ << + ", Product ID: " << device_info.product_id_ << + ")" << std::endl; + return stream; + } + + } //namespace + } +} diff --git a/io/src/openni2/openni2_device_manager.cpp b/io/src/openni2/openni2_device_manager.cpp new file mode 100644 index 00000000000..4177a1a066b --- /dev/null +++ b/io/src/openni2/openni2_device_manager.cpp @@ -0,0 +1,266 @@ +/* + * Copyright (c) 2013, Willow Garage, 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 Willow Garage, Inc. 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. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#include "pcl/io/openni2/openni2_device_manager.h" +#include "pcl/io/openni2/openni2_convert.h" +#include "pcl/io/openni2/openni2_device.h" +#include "pcl/io/io_exception.h" + +#include + +#include +#include + +#include "OpenNI.h" + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + class OpenNI2DeviceInfoComparator + { + public: + bool operator ()(const OpenNI2DeviceInfo& di1, const OpenNI2DeviceInfo& di2) + { + return (di1.uri_.compare (di2.uri_) < 0); + } + }; + + typedef std::set DeviceSet; + + + + class OpenNI2DeviceListener : public openni::OpenNI::DeviceConnectedListener, + public openni::OpenNI::DeviceDisconnectedListener, + public openni::OpenNI::DeviceStateChangedListener + { + public: + OpenNI2DeviceListener () + : openni::OpenNI::DeviceConnectedListener () + , openni::OpenNI::DeviceDisconnectedListener () + , openni::OpenNI::DeviceStateChangedListener () + { + openni::OpenNI::addDeviceConnectedListener (this); + openni::OpenNI::addDeviceDisconnectedListener (this); + openni::OpenNI::addDeviceStateChangedListener (this); + + // get list of currently connected devices + openni::Array device_info_list; + openni::OpenNI::enumerateDevices (&device_info_list); + + for (int i = 0; i < device_info_list.getSize (); ++i) + { + onDeviceConnected (&device_info_list[i]); + } + } + + ~OpenNI2DeviceListener () + { + openni::OpenNI::removeDeviceConnectedListener (this); + openni::OpenNI::removeDeviceDisconnectedListener (this); + openni::OpenNI::removeDeviceStateChangedListener (this); + } + + virtual void + onDeviceStateChanged (const openni::DeviceInfo* pInfo, openni::DeviceState state) + { + switch (state) + { + case openni::DEVICE_STATE_OK: + onDeviceConnected (pInfo); + break; + case openni::DEVICE_STATE_ERROR: + case openni::DEVICE_STATE_NOT_READY: + case openni::DEVICE_STATE_EOF: + default: + onDeviceDisconnected (pInfo); + break; + } + } + + virtual void + onDeviceConnected (const openni::DeviceInfo* pInfo) + { + boost::mutex::scoped_lock l (device_mutex_); + + const OpenNI2DeviceInfo device_info_wrapped = openni2_convert (pInfo); + + // make sure it does not exist in set before inserting + device_set_.erase (device_info_wrapped); + device_set_.insert (device_info_wrapped); + } + + virtual void + onDeviceDisconnected (const openni::DeviceInfo* pInfo) + { + boost::mutex::scoped_lock l (device_mutex_); + + const OpenNI2DeviceInfo device_info_wrapped = openni2_convert (pInfo); + device_set_.erase (device_info_wrapped); + } + + boost::shared_ptr > + getConnectedDeviceURIs () + { + boost::mutex::scoped_lock l (device_mutex_); + + boost::shared_ptr > result = boost::make_shared >(); + + result->reserve (device_set_.size ()); + + std::set::const_iterator it; + std::set::const_iterator it_end = device_set_.end (); + + for (it = device_set_.begin (); it != it_end; ++it) + result->push_back (it->uri_); + + return result; + } + + boost::shared_ptr > + getConnectedDeviceInfos () + { + boost::mutex::scoped_lock l (device_mutex_); + + boost::shared_ptr > result = boost::make_shared >(); + + result->reserve (device_set_.size ()); + + DeviceSet::const_iterator it; + DeviceSet::const_iterator it_end = device_set_.end (); + + for (it = device_set_.begin (); it != it_end; ++it) + result->push_back (*it); + + return result; + } + + std::size_t + getNumOfConnectedDevices () + { + boost::mutex::scoped_lock l (device_mutex_); + + return device_set_.size (); + } + + boost::mutex device_mutex_; + DeviceSet device_set_; + }; + + } //namespace + } +} + + +////////////////////////////////////////////////////////////////////////// +using pcl::io::openni2::OpenNI2Device; +using pcl::io::openni2::OpenNI2DeviceInfo; +using pcl::io::openni2::OpenNI2DeviceManager; + +pcl::io::openni2::OpenNI2DeviceManager::OpenNI2DeviceManager () +{ + openni::Status rc = openni::OpenNI::initialize (); + if (rc != openni::STATUS_OK) + THROW_IO_EXCEPTION ("Initialize failed\n%s\n", openni::OpenNI::getExtendedError ()); + + device_listener_ = boost::make_shared(); +} + +pcl::io::openni2::OpenNI2DeviceManager::~OpenNI2DeviceManager () +{ +} + +boost::shared_ptr > +pcl::io::openni2::OpenNI2DeviceManager::getConnectedDeviceInfos () const +{ + return device_listener_->getConnectedDeviceInfos (); +} + +boost::shared_ptr > +pcl::io::openni2::OpenNI2DeviceManager::getConnectedDeviceURIs () const +{ + return device_listener_->getConnectedDeviceURIs (); +} + +std::size_t +pcl::io::openni2::OpenNI2DeviceManager::getNumOfConnectedDevices () const +{ + return device_listener_->getNumOfConnectedDevices (); +} + +boost::shared_ptr +pcl::io::openni2::OpenNI2DeviceManager::getAnyDevice () +{ + return boost::make_shared(""); +} + +boost::shared_ptr +pcl::io::openni2::OpenNI2DeviceManager::getDevice (const std::string& device_URI) +{ + return boost::make_shared(device_URI); +} + +boost::shared_ptr +pcl::io::openni2::OpenNI2DeviceManager::getDeviceByIndex (int index) +{ + boost::shared_ptr > URIs = getConnectedDeviceURIs (); + return boost::make_shared( URIs->at (index) ); +} + +boost::shared_ptr +pcl::io::openni2::OpenNI2DeviceManager::getFileDevice (const std::string& path) +{ + return boost::make_shared(path); +} + +std::ostream& +operator<< (std::ostream& stream, const OpenNI2DeviceManager& device_manager) +{ + + boost::shared_ptr > device_info = device_manager.getConnectedDeviceInfos (); + + std::vector::const_iterator it; + std::vector::const_iterator it_end = device_info->end (); + + for (it = device_info->begin (); it != it_end; ++it) + { + stream << "Uri: " << it->uri_ << " (Vendor: " << it->vendor_ << + ", Name: " << it->name_ << + ", Vendor ID: " << it->vendor_id_ << + ", Product ID: " << it->product_id_ << + ")" << std::endl; + } + + return (stream); +} diff --git a/io/src/openni2/openni2_timer_filter.cpp b/io/src/openni2/openni2_timer_filter.cpp new file mode 100644 index 00000000000..43fbe6c8c9d --- /dev/null +++ b/io/src/openni2/openni2_timer_filter.cpp @@ -0,0 +1,98 @@ +/* + * Copyright (c) 2013, Willow Garage, 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 Willow Garage, Inc. 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. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#include "pcl/io/openni2/openni2_timer_filter.h" +#include + + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + OpenNI2TimerFilter::OpenNI2TimerFilter (std::size_t filter_len): + filter_len_(filter_len) + {} + + OpenNI2TimerFilter::~OpenNI2TimerFilter () + {} + + void OpenNI2TimerFilter::addSample (double sample) + { + buffer_.push_back (sample); + if (buffer_.size ()>filter_len_) + buffer_.pop_front (); + } + + double OpenNI2TimerFilter::getMedian () + { + if (buffer_.size ()>0) + { + std::deque sort_buffer = buffer_; + + std::sort (sort_buffer.begin (), sort_buffer.end ()); + + return sort_buffer[sort_buffer.size ()/2]; + } else + return (0.0); + } + + double + OpenNI2TimerFilter::getMovingAvg () + { + if (buffer_.size () > 0) + { + double sum = 0; + + std::deque::const_iterator it = buffer_.begin (); + std::deque::const_iterator it_end = buffer_.end (); + + while (it != it_end) + { + sum += *(it++); + } + + return sum / static_cast(buffer_.size ()); + } else + return (0.0); + } + + + void OpenNI2TimerFilter::clear () + { + buffer_.clear (); + } + + } //namespace + } +} diff --git a/io/src/openni2/openni2_video_mode.cpp b/io/src/openni2/openni2_video_mode.cpp new file mode 100644 index 00000000000..145da5d3851 --- /dev/null +++ b/io/src/openni2/openni2_video_mode.cpp @@ -0,0 +1,102 @@ +/* + * Copyright (c) 2013, Willow Garage, 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 Willow Garage, Inc. 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. + * + * Author: Julius Kammerl (jkammerl@willowgarage.com) + */ + +#include "pcl/io/openni2/openni2_video_mode.h" + +namespace pcl +{ + namespace io + { + namespace openni2 + { + + std::ostream& + operator<< (std::ostream& stream, const OpenNI2VideoMode& video_mode) + { + stream << "Resolution: " << (int)video_mode.x_resolution_ << "x" << (int)video_mode.y_resolution_ << + "@" << video_mode.frame_rate_ << + "Hz Format: "; + + switch (video_mode.pixel_format_) + { + case PIXEL_FORMAT_DEPTH_1_MM: + stream << "Depth 1mm"; + break; + case PIXEL_FORMAT_DEPTH_100_UM: + stream << "Depth 100um"; + break; + case PIXEL_FORMAT_SHIFT_9_2: + stream << "Shift 9/2"; + break; + case PIXEL_FORMAT_SHIFT_9_3: + stream << "Shift 9/3"; + break; + case PIXEL_FORMAT_RGB888: + stream << "RGB888"; + break; + case PIXEL_FORMAT_YUV422: + stream << "YUV422"; + break; + case PIXEL_FORMAT_GRAY8: + stream << "Gray8"; + break; + case PIXEL_FORMAT_GRAY16: + stream << "Gray16"; + break; + case PIXEL_FORMAT_JPEG: + stream << "JPEG"; + break; + + default: + break; + } + + return (stream); + } + + bool + operator==(const OpenNI2VideoMode& video_mode_a, const OpenNI2VideoMode& video_mode_b) + { + return (video_mode_a.x_resolution_==video_mode_b.x_resolution_) && + (video_mode_a.y_resolution_==video_mode_b.y_resolution_) && + (video_mode_a.frame_rate_ ==video_mode_b.frame_rate_) && + (video_mode_a.pixel_format_==video_mode_b.pixel_format_); + } + + bool + operator!=(const OpenNI2VideoMode& video_mode_a, const OpenNI2VideoMode& video_mode_b) + { + return !(video_mode_a==video_mode_b); + } + + } //namespace + } +} diff --git a/io/src/openni2_grabber.cpp b/io/src/openni2_grabber.cpp new file mode 100644 index 00000000000..c05a768c2ab --- /dev/null +++ b/io/src/openni2_grabber.cpp @@ -0,0 +1,954 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * Copyright (c) 2014, respective authors. + * + * 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 +#ifdef HAVE_OPENNI2 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace pcl::io::openni2; + +namespace pcl +{ + typedef union + { + struct + { + unsigned char Blue; + unsigned char Green; + unsigned char Red; + unsigned char Alpha; + }; + float float_value; + uint32_t long_value; + } RGBValue; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +pcl::io::OpenNI2Grabber::OpenNI2Grabber (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode) + : rgb_sync_ () + , ir_sync_ () + , device_ () + , rgb_frame_id_ () + , depth_frame_id_ () + , image_width_ () + , image_height_ () + , depth_width_ () + , depth_height_ () + , image_required_ (false) + , depth_required_ (false) + , ir_required_ (false) + , sync_required_ (false) + , image_signal_ (), depth_image_signal_ (), ir_image_signal_ (), image_depth_image_signal_ () + , ir_depth_image_signal_ (), point_cloud_signal_ (), point_cloud_i_signal_ () + , point_cloud_rgb_signal_ (), point_cloud_rgba_signal_ () + , config2oni_map_ (), depth_callback_handle_ (), image_callback_handle_ (), ir_callback_handle_ () + , running_ (false) + , rgb_parameters_(std::numeric_limits::quiet_NaN () ) + , depth_parameters_(std::numeric_limits::quiet_NaN () ) +{ + // initialize driver + updateModeMaps (); // registering mapping from PCL enum modes to openni::VideoMode and vice versa + setupDevice (device_id, depth_mode, image_mode); + + rgb_frame_id_ = "/openni2_rgb_optical_frame"; + depth_frame_id_ = "/openni2_depth_optical_frame"; + + + if (!device_->hasDepthSensor () ) + PCL_THROW_EXCEPTION (pcl::IOException, "Device does not provide 3D information."); + + depth_image_signal_ = createSignal (); + ir_image_signal_ = createSignal (); + point_cloud_signal_ = createSignal (); + point_cloud_i_signal_ = createSignal (); + ir_depth_image_signal_ = createSignal (); + ir_sync_.addCallback (boost::bind (&OpenNI2Grabber::irDepthImageCallback, this, _1, _2)); + + if (device_->hasColorSensor ()) + { + // create callback signals + image_signal_ = createSignal (); + image_depth_image_signal_ = createSignal (); + point_cloud_rgb_signal_ = createSignal (); + point_cloud_rgba_signal_ = createSignal (); + rgb_sync_.addCallback (boost::bind (&OpenNI2Grabber::imageDepthImageCallback, this, _1, _2)); + } + + // Register callbacks from the sensor to the grabber + device_->setColorCallback (boost::bind (&OpenNI2Grabber::processColorFrame, this, _1)); + device_->setDepthCallback (boost::bind (&OpenNI2Grabber::processDepthFrame, this, _1)); + device_->setIRCallback (boost::bind (&OpenNI2Grabber::processIRFrame, this, _1)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +pcl::io::OpenNI2Grabber::~OpenNI2Grabber () throw () +{ + try + { + stop (); + + // release the pointer to the device object + device_.reset (); + + // disconnect all listeners + disconnect_all_slots (); + disconnect_all_slots (); + disconnect_all_slots (); + disconnect_all_slots (); + disconnect_all_slots (); + disconnect_all_slots (); + disconnect_all_slots (); + disconnect_all_slots (); + } + catch (...) + { + // destructor never throws + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::checkImageAndDepthSynchronizationRequired () +{ + // do we have anyone listening to images or color point clouds? + if (num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0) + sync_required_ = true; + else + sync_required_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::checkImageStreamRequired () +{ + // do we have anyone listening to images or color point clouds? + if (num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0) + image_required_ = true; + else + image_required_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::checkDepthStreamRequired () +{ + // do we have anyone listening to depth images or (color) point clouds? + if (num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0 ) + depth_required_ = true; + else + depth_required_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::checkIRStreamRequired () +{ + if (num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0) + ir_required_ = true; + else + ir_required_ = false; +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::start () +{ + try + { + // check if we need to start/stop any stream + if (image_required_ && !device_->isColorStreamStarted () ) + { + block_signals (); + device_->startColorStream (); + startSynchronization (); + } + + if (depth_required_ && !device_->isDepthStreamStarted ()) + { + block_signals (); + if (device_->hasColorSensor () && device_->isImageRegistrationModeSupported () ) + { + device_->setImageRegistrationMode (true); + } + device_->startDepthStream (); + startSynchronization (); + } + + if (ir_required_ && !device_->isIRStreamStarted () ) + { + block_signals (); + device_->startIRStream (); + } + running_ = true; + } + catch (IOException& ex) + { + PCL_THROW_EXCEPTION (pcl::IOException, "Could not start streams. Reason: " << ex.what ()); + } + + // workaround, since the first frame is corrupted + //boost::this_thread::sleep (boost::posix_time::seconds (1)); + unblock_signals (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::stop () +{ + try + { + if (device_->hasDepthSensor () && device_->isDepthStreamStarted () ) + device_->stopDepthStream (); + + if (device_->hasColorSensor () && device_->isColorStreamStarted () ) + device_->stopColorStream (); + + if (device_->hasIRSensor () && device_->isIRStreamStarted ()) + device_->stopIRStream (); + + running_ = false; + } + catch (IOException& ex) + { + PCL_THROW_EXCEPTION (pcl::IOException, "Could not stop streams. Reason: " << ex.what ()); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl::io::OpenNI2Grabber::isRunning () const +{ + return (running_); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::signalsChanged () +{ + // reevaluate which streams are required + checkImageStreamRequired (); + checkDepthStreamRequired (); + checkIRStreamRequired (); + if (ir_required_ && image_required_) + PCL_THROW_EXCEPTION (pcl::IOException, "Can not provide IR stream and RGB stream at the same time."); + + checkImageAndDepthSynchronizationRequired (); + if (running_) + start (); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +std::string +pcl::io::OpenNI2Grabber::getName () const +{ + return (std::string ("OpenNI2Grabber")); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::setupDevice (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode) +{ + // Initialize the openni device + boost::shared_ptr deviceManager = OpenNI2DeviceManager::getInstance (); + + try + { + if (boost::filesystem::exists (device_id)) + { + device_ = deviceManager->getFileDevice (device_id); // Treat as file path + } + else if (deviceManager->getNumOfConnectedDevices () == 0) + { + PCL_THROW_EXCEPTION (pcl::IOException, "No devices connected."); + } + else if (device_id[0] == '#') + { + unsigned index = atoi (device_id.c_str () + 1); + device_ = deviceManager->getDeviceByIndex (index - 1); + } + else + { + device_ = deviceManager->getAnyDevice (); + } + } + catch (const IOException& exception) + { + if (!device_) + PCL_THROW_EXCEPTION (pcl::IOException, "No matching device found. " << exception.what ()) + else + PCL_THROW_EXCEPTION (pcl::IOException, "could not retrieve device. Reason " << exception.what ()) + } + catch (const pcl::IOException&) + { + throw; + } + catch (...) + { + PCL_THROW_EXCEPTION (pcl::IOException, "unknown error occured"); + } + + typedef pcl::io::openni2::OpenNI2VideoMode VideoMode; + + VideoMode depth_md; + // Set the selected output mode + if (depth_mode != OpenNI_Default_Mode) + { + VideoMode actual_depth_md; + if (!mapMode2XnMode (depth_mode, depth_md) || !device_->findCompatibleDepthMode (depth_md, actual_depth_md)) + PCL_THROW_EXCEPTION (pcl::IOException, "could not find compatible depth stream mode " << static_cast (depth_mode) ); + + VideoMode current_depth_md = device_->getDepthVideoMode (); + if (current_depth_md.x_resolution_ != actual_depth_md.x_resolution_ || current_depth_md.y_resolution_ != actual_depth_md.y_resolution_) + device_->setDepthVideoMode (actual_depth_md); + } + else + { + depth_md = device_->getDefaultDepthMode (); + } + + depth_width_ = depth_md.x_resolution_; + depth_height_ = depth_md.y_resolution_; + + if (device_->hasColorSensor ()) + { + VideoMode image_md; + if (image_mode != OpenNI_Default_Mode) + { + VideoMode actual_image_md; + if (!mapMode2XnMode (image_mode, image_md) || !device_->findCompatibleColorMode (image_md, actual_image_md)) + PCL_THROW_EXCEPTION (pcl::IOException, "could not find compatible image stream mode " << static_cast (image_mode) ); + + VideoMode current_image_md = device_->getColorVideoMode (); + if (current_image_md.x_resolution_ != actual_image_md.x_resolution_ || current_image_md.y_resolution_ != actual_image_md.y_resolution_) + device_->setColorVideoMode (actual_image_md); + } + else + { + image_md = device_->getDefaultColorMode (); + } + + image_width_ = image_md.x_resolution_; + image_height_ = image_md.y_resolution_; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::startSynchronization () +{ + try + { + if (device_->isSynchronizationSupported () && !device_->isSynchronized () && !device_->isFile () && + device_->getColorVideoMode ().frame_rate_ == device_->getDepthVideoMode ().frame_rate_) + device_->setSynchronization (true); + } + catch (const IOException& exception) + { + PCL_THROW_EXCEPTION (pcl::IOException, "Could not start synchronization " << exception.what ()); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::stopSynchronization () +{ + try + { + if (device_->isSynchronizationSupported () && device_->isSynchronized ()) + device_->setSynchronization (false); + } + catch (const IOException& exception) + { + PCL_THROW_EXCEPTION (pcl::IOException, "Could not start synchronization " << exception.what ()); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::imageCallback (Image::Ptr image, void*) +{ + if (num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0) + rgb_sync_.add0 (image, image->getTimestamp ()); + + int numImageSlots = image_signal_->num_slots (); + if (numImageSlots > 0) + image_signal_->operator ()(image); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::depthCallback (DepthImage::Ptr depth_image, void*) +{ + if (num_slots () > 0 || + num_slots () > 0 || + num_slots () > 0) + rgb_sync_.add1 (depth_image, depth_image->getTimestamp ()); + + if (num_slots () > 0 || + num_slots () > 0) + ir_sync_.add1 (depth_image, depth_image->getTimestamp ()); + + if (depth_image_signal_->num_slots () > 0) + depth_image_signal_->operator ()(depth_image); + + if (point_cloud_signal_->num_slots () > 0) + point_cloud_signal_->operator ()(convertToXYZPointCloud (depth_image)); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::irCallback (IRImage::Ptr ir_image, void*) +{ + if (num_slots () > 0 || + num_slots () > 0) + ir_sync_.add0(ir_image, ir_image->getTimestamp ()); + + if (ir_image_signal_->num_slots () > 0) + ir_image_signal_->operator ()(ir_image); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::imageDepthImageCallback (const Image::Ptr &image, + const DepthImage::Ptr &depth_image) +{ + // check if we have color point cloud slots + if (point_cloud_rgb_signal_->num_slots () > 0) + { + PCL_WARN ("PointXYZRGB callbacks deprecated. Use PointXYZRGBA instead.\n"); + point_cloud_rgb_signal_->operator ()(convertToXYZRGBPointCloud (image, depth_image)); + } + + if (point_cloud_rgba_signal_->num_slots () > 0) + point_cloud_rgba_signal_->operator ()(convertToXYZRGBPointCloud (image, depth_image)); + + if (image_depth_image_signal_->num_slots () > 0) + { + float reciprocalFocalLength = 1.0f / device_->getDepthFocalLength (depth_width_); + image_depth_image_signal_->operator ()(image, depth_image, reciprocalFocalLength); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::irDepthImageCallback (const IRImage::Ptr &ir_image, + const DepthImage::Ptr &depth_image) +{ + // check if we have color point cloud slots + if (point_cloud_i_signal_->num_slots () > 0) + point_cloud_i_signal_->operator ()(convertToXYZIPointCloud (ir_image, depth_image)); + + if (ir_depth_image_signal_->num_slots () > 0) + { + float reciprocalFocalLength = 1.0f / device_->getDepthFocalLength (depth_width_); + ir_depth_image_signal_->operator ()(ir_image, depth_image, reciprocalFocalLength); + } +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +pcl::PointCloud::Ptr +pcl::io::OpenNI2Grabber::convertToXYZPointCloud (const DepthImage::Ptr& depth_image) const +{ + pcl::PointCloud::Ptr cloud (new pcl::PointCloud ); + + cloud->height = depth_height_; + cloud->width = depth_width_; + cloud->is_dense = false; + + cloud->points.resize (cloud->height * cloud->width); + + register float constant_x = 1.0f / device_->getDepthFocalLength (depth_width_); + register float constant_y = 1.0f / device_->getDepthFocalLength (depth_width_); + register float centerX = ((float)cloud->width - 1.f) / 2.f; + register float centerY = ((float)cloud->height - 1.f) / 2.f; + + if (pcl_isfinite (depth_parameters_.focal_length_x)) + constant_x = 1.0f / static_cast (depth_parameters_.focal_length_x); + + if (pcl_isfinite (depth_parameters_.focal_length_y)) + constant_y = 1.0f / static_cast (depth_parameters_.focal_length_y); + + if (pcl_isfinite (depth_parameters_.principal_point_x)) + centerX = static_cast (depth_parameters_.principal_point_x); + + if (pcl_isfinite (depth_parameters_.principal_point_y)) + centerY = static_cast (depth_parameters_.principal_point_y); + + //if (device_->isDepthRegistered ()) + if (true) + cloud->header.frame_id = rgb_frame_id_; + else + cloud->header.frame_id = depth_frame_id_; + + + float bad_point = std::numeric_limits::quiet_NaN (); + + // we have to use Data, since operator[] uses assert -> Debug-mode very slow! + register const unsigned short* depth_map = (const unsigned short*) depth_image->getData (); + if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_) + { + static unsigned buffer_size = 0; + static boost::shared_array depth_buffer ((unsigned short*)(NULL)); + + if (buffer_size < depth_width_ * depth_height_) + { + buffer_size = depth_width_ * depth_height_; + depth_buffer.reset (new unsigned short [buffer_size]); + } + depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ()); + depth_map = depth_buffer.get (); + } + + register int depth_idx = 0; + for (int v = 0; v < depth_height_; ++v) + { + for (register int u = 0; u < depth_width_; ++u, ++depth_idx) + { + pcl::PointXYZ& pt = cloud->points[depth_idx]; + // Check for invalid measurements + if (depth_map[depth_idx] == 0 || + depth_map[depth_idx] == depth_image->getNoSampleValue () || + depth_map[depth_idx] == depth_image->getShadowValue ()) + { + // not valid + pt.x = pt.y = pt.z = bad_point; + continue; + } + pt.z = depth_map[depth_idx] * 0.001f; + pt.x = (static_cast (u) - centerX) * pt.z * constant_x; + pt.y = (static_cast (v) - centerY) * pt.z * constant_y; + } + } + cloud->sensor_origin_.setZero (); + cloud->sensor_orientation_.w () = 1.0f; + cloud->sensor_orientation_.x () = 0.0f; + cloud->sensor_orientation_.y () = 0.0f; + cloud->sensor_orientation_.z () = 0.0f; + return (cloud); +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template typename pcl::PointCloud::Ptr +pcl::io::OpenNI2Grabber::convertToXYZRGBPointCloud (const Image::Ptr &image, + const DepthImage::Ptr &depth_image) const +{ + static unsigned rgb_array_size = 0; + static boost::shared_array rgb_array ((unsigned char*)(NULL)); + static unsigned char* rgb_buffer = 0; + + boost::shared_ptr > cloud (new pcl::PointCloud); + + cloud->header.frame_id = rgb_frame_id_; + cloud->height = std::max (image_height_, depth_height_); + cloud->width = std::max (image_width_, depth_width_); + cloud->is_dense = false; + + cloud->points.resize (cloud->height * cloud->width); + + // Generate default camera parameters + float fx = device_->getColorFocalLength (depth_width_); // Horizontal focal length + float fy = device_->getColorFocalLength (depth_width_); // Vertcal focal length + register float cx = ((float)cloud->width - 1.f) / 2.f; // Center x + register float cy = ((float)cloud->height - 1.f) / 2.f; // Center y + + // Load pre-calibrated camera parameters if they exist + if (pcl_isfinite (depth_parameters_.focal_length_x)) + fx = 1.0f / static_cast (depth_parameters_.focal_length_x); + + if (pcl_isfinite (depth_parameters_.focal_length_y)) + fy = 1.0f / static_cast (depth_parameters_.focal_length_y); + + if (pcl_isfinite (depth_parameters_.principal_point_x)) + cx = static_cast (depth_parameters_.principal_point_x); + + if (pcl_isfinite (depth_parameters_.principal_point_y)) + cy = static_cast (depth_parameters_.principal_point_y); + + // Get inverse focal length for calculations below + register float fx_inv = 1.0f / fx; + register float fy_inv = 1.0f / fy; + + register const OniDepthPixel* depth_map = (OniDepthPixel*) depth_image->getData (); + if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_) + { + static unsigned buffer_size = 0; + static boost::shared_array depth_buffer ((unsigned short*)(NULL)); + + if (buffer_size < depth_width_ * depth_height_) + { + buffer_size = depth_width_ * depth_height_; + depth_buffer.reset (new unsigned short [buffer_size]); + } + + depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ()); + depth_map = depth_buffer.get (); + } + + // here we need exact the size of the point cloud for a one-one correspondence! + if (rgb_array_size < image_width_ * image_height_ * 3) + { + rgb_array_size = image_width_ * image_height_ * 3; + rgb_array.reset (new unsigned char [rgb_array_size]); + rgb_buffer = rgb_array.get (); + } + image->fillRGB (image_width_, image_height_, rgb_buffer, image_width_ * 3); + float bad_point = std::numeric_limits::quiet_NaN (); + + // set xyz to Nan and rgb to 0 (black) + if (image_width_ != depth_width_) + { + PointT pt; + pt.x = pt.y = pt.z = bad_point; + pt.b = pt.g = pt.r = 0; + pt.a = 255; // point has no color info -> alpha = max => transparent + cloud->points.assign (cloud->points.size (), pt); + } + + // fill in XYZ values + unsigned step = cloud->width / depth_width_; + unsigned skip = cloud->width * step - cloud->width; + + int value_idx = 0; + int point_idx = 0; + for (int v = 0; v < depth_height_; ++v, point_idx += skip) + { + for (register int u = 0; u < depth_width_; ++u, ++value_idx, point_idx += step) + { + PointT& pt = cloud->points[point_idx]; + /// @todo Different values for these cases + // Check for invalid measurements + + OniDepthPixel pixel = depth_map[value_idx]; + if (pixel != 0 && + pixel != depth_image->getNoSampleValue () && + pixel != depth_image->getShadowValue () ) + { + pt.z = depth_map[value_idx] * 0.001f; // millimeters to meters + pt.x = (static_cast (u) - cx) * pt.z * fx_inv; + pt.y = (static_cast (v) - cy) * pt.z * fy_inv; + } + else + { + pt.x = pt.y = pt.z = bad_point; + } + } + } + + // fill in the RGB values + step = cloud->width / image_width_; + skip = cloud->width * step - cloud->width; + + value_idx = 0; + point_idx = 0; + RGBValue color; + color.Alpha = 0; + + for (unsigned yIdx = 0; yIdx < image_height_; ++yIdx, point_idx += skip) + { + for (unsigned xIdx = 0; xIdx < image_width_; ++xIdx, point_idx += step, value_idx += 3) + { + PointT& pt = cloud->points[point_idx]; + + color.Red = rgb_buffer[value_idx]; + color.Green = rgb_buffer[value_idx + 1]; + color.Blue = rgb_buffer[value_idx + 2]; + + pt.rgba = color.long_value; + } + } + cloud->sensor_origin_.setZero (); + cloud->sensor_orientation_.setIdentity (); + return (cloud); +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +pcl::PointCloud::Ptr +pcl::io::OpenNI2Grabber::convertToXYZIPointCloud (const IRImage::Ptr &ir_image, + const DepthImage::Ptr &depth_image) const +{ + boost::shared_ptr > cloud (new pcl::PointCloud ()); + + cloud->header.frame_id = rgb_frame_id_; + cloud->height = depth_height_; + cloud->width = depth_width_; + cloud->is_dense = false; + + cloud->points.resize (cloud->height * cloud->width); + + + float fx = device_->getColorFocalLength (depth_width_); // Horizontal focal length + float fy = device_->getColorFocalLength (depth_width_); // Vertcal focal length + register float cx = ((float)cloud->width - 1.f) / 2.f; // Center x + register float cy = ((float)cloud->height - 1.f) / 2.f; // Center y + + // Load pre-calibrated camera parameters if they exist + if (pcl_isfinite (depth_parameters_.focal_length_x)) + fx = 1.0f / static_cast (depth_parameters_.focal_length_x); + + if (pcl_isfinite (depth_parameters_.focal_length_y)) + fy = 1.0f / static_cast (depth_parameters_.focal_length_y); + + if (pcl_isfinite (depth_parameters_.principal_point_x)) + cx = static_cast (depth_parameters_.principal_point_x); + + if (pcl_isfinite (depth_parameters_.principal_point_y)) + cy = static_cast (depth_parameters_.principal_point_y); + + register float fx_inv = 1.0f / fx; + register float fy_inv = 1.0f / fy; + + + + register const unsigned short* depth_map = (const unsigned short*) depth_image->getData (); + register const unsigned short* ir_map = ir_image->getData (); + + if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_) + { + static unsigned buffer_size = 0; + static boost::shared_array depth_buffer ((unsigned short*)(NULL)); + static boost::shared_array ir_buffer ((unsigned short*)(NULL)); + + if (buffer_size < depth_width_ * depth_height_) + { + buffer_size = depth_width_ * depth_height_; + depth_buffer.reset (new unsigned short [buffer_size]); + ir_buffer.reset (new unsigned short [buffer_size]); + } + + depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ()); + depth_map = depth_buffer.get (); + + ir_image->fillRaw (depth_width_, depth_height_, ir_buffer.get ()); + ir_map = ir_buffer.get (); + } + + register int depth_idx = 0; + float bad_point = std::numeric_limits::quiet_NaN (); + + for (int v = 0; v < depth_height_; ++v) + { + for (register int u = 0; u < depth_width_; ++u, ++depth_idx) + { + pcl::PointXYZI& pt = cloud->points[depth_idx]; + /// @todo Different values for these cases + // Check for invalid measurements + if (depth_map[depth_idx] == 0 || + depth_map[depth_idx] == depth_image->getNoSampleValue () || + depth_map[depth_idx] == depth_image->getShadowValue ()) + { + pt.x = pt.y = pt.z = bad_point; + } + else + { + pt.z = depth_map[depth_idx] * 0.001f; // millimeters to meters + pt.x = (static_cast (u) - cx) * pt.z * fx; + pt.y = (static_cast (v) - cy) * pt.z * fy; + } + + pt.data_c[0] = pt.data_c[1] = pt.data_c[2] = pt.data_c[3] = 0; + pt.intensity = static_cast (ir_map[depth_idx]); + } + } + cloud->sensor_origin_.setZero (); + cloud->sensor_orientation_.setIdentity (); + return (cloud); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +void +pcl::io::OpenNI2Grabber::updateModeMaps () +{ + typedef pcl::io::openni2::OpenNI2VideoMode VideoMode; + +pcl::io::openni2::OpenNI2VideoMode output_mode; + + config2oni_map_[OpenNI_SXGA_15Hz] = VideoMode (XN_SXGA_X_RES, XN_SXGA_Y_RES, 15); + + config2oni_map_[OpenNI_VGA_25Hz] = VideoMode (XN_VGA_X_RES, XN_VGA_Y_RES, 25); + config2oni_map_[OpenNI_VGA_30Hz] = VideoMode (XN_VGA_X_RES, XN_VGA_Y_RES, 30); + + config2oni_map_[OpenNI_QVGA_25Hz] = VideoMode (XN_QVGA_X_RES, XN_QVGA_Y_RES, 25); + config2oni_map_[OpenNI_QVGA_30Hz] = VideoMode (XN_QVGA_X_RES, XN_QVGA_Y_RES, 30); + config2oni_map_[OpenNI_QVGA_60Hz] = VideoMode (XN_QVGA_X_RES, XN_QVGA_Y_RES, 60); + + config2oni_map_[OpenNI_QQVGA_25Hz] = VideoMode (XN_QQVGA_X_RES, XN_QQVGA_Y_RES, 25); + config2oni_map_[OpenNI_QQVGA_30Hz] = VideoMode (XN_QQVGA_X_RES, XN_QQVGA_Y_RES, 30); + config2oni_map_[OpenNI_QQVGA_60Hz] = VideoMode (XN_QQVGA_X_RES, XN_QQVGA_Y_RES, 60); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl::io::OpenNI2Grabber::mapMode2XnMode (int mode, OpenNI2VideoMode &xnmode) const +{ + std::map::const_iterator it = config2oni_map_.find (mode); + if (it != config2oni_map_.end ()) + { + xnmode = it->second; + return (true); + } + else + { + return (false); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +std::vector > +pcl::io::OpenNI2Grabber::getAvailableDepthModes () const +{ +pcl::io::openni2::OpenNI2VideoMode dummy; + std::vector > result; + for (std::map::const_iterator it = config2oni_map_.begin (); it != config2oni_map_.end (); ++it) + { + if (device_->findCompatibleDepthMode (it->second, dummy)) + result.push_back (*it); + } + + return (result); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +std::vector > +pcl::io::OpenNI2Grabber::getAvailableImageModes () const +{ +pcl::io::openni2::OpenNI2VideoMode dummy; + std::vector > result; + for (std::map::const_iterator it = config2oni_map_.begin (); it != config2oni_map_.end (); ++it) + { + if (device_->findCompatibleColorMode (it->second, dummy)) + result.push_back (*it); + } + + return (result); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +float +pcl::io::OpenNI2Grabber::getFramesPerSecond () const +{ + return (static_cast (device_->getColorVideoMode ().frame_rate_)); +} + + + +// Convert VideoFrameRef into pcl::Image and forward to registered callbacks +void pcl::io::OpenNI2Grabber::processColorFrame (openni::VideoStream& stream) +{ + Image::Timestamp t_callback = Image::Clock::now (); + + openni::VideoFrameRef frame; + stream.readFrame (&frame); + FrameWrapper::Ptr frameWrapper = boost::make_shared(frame); + + openni::PixelFormat format = frame.getVideoMode ().getPixelFormat (); + boost::shared_ptr image; + + // Convert frame to PCL image type, based on pixel format + if (format == openni::PIXEL_FORMAT_YUV422) + image = boost::make_shared (frameWrapper, t_callback); + else //if (format == PixelFormat::PIXEL_FORMAT_RGB888) + image = boost::make_shared (frameWrapper, t_callback); + + imageCallback (image, NULL); +} + + +void pcl::io::OpenNI2Grabber::processDepthFrame (openni::VideoStream& stream) +{ + openni::VideoFrameRef frame; + stream.readFrame (&frame); + FrameWrapper::Ptr frameWrapper = boost::make_shared(frame); + + boost::posix_time::ptime t_readFrameTimestamp = boost::posix_time::microsec_clock::local_time (); + int frameWidth = frame.getWidth (); + float focalLength = device_->getDepthFocalLength (frameWidth); + + float baseline = device_->getBaseline(); + uint64_t no_sample_value = device_->getShadowValue(); + uint64_t shadow_value = no_sample_value; + + boost::shared_ptr image = + boost::make_shared (frameWrapper, baseline, focalLength, shadow_value, no_sample_value); + + depthCallback (image, NULL); +} + + +void pcl::io::OpenNI2Grabber::processIRFrame (openni::VideoStream& stream) +{ + openni::VideoFrameRef frame; + stream.readFrame (&frame); + boost::posix_time::ptime t_readFrameTimestamp = boost::posix_time::microsec_clock::local_time (); + + FrameWrapper::Ptr frameWrapper = boost::make_shared(frame); + + boost::shared_ptr image = boost::make_shared ( frameWrapper ); + + irCallback (image, NULL); +} + +#endif // HAVE_OPENNI2 From 7da8ed544c0365ab3bb2885a91a52a20187dacc3 Mon Sep 17 00:00:00 2001 From: Ky Waegel Date: Mon, 3 Mar 2014 17:35:08 -0500 Subject: [PATCH 06/12] Stopped throwing exception on frame sync failure. * This is not an exceptional event and always occurs for file devices. Print error message instead. --- io/src/openni2_grabber.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/io/src/openni2_grabber.cpp b/io/src/openni2_grabber.cpp index c05a768c2ab..94a235b1de6 100644 --- a/io/src/openni2_grabber.cpp +++ b/io/src/openni2_grabber.cpp @@ -403,7 +403,7 @@ pcl::io::OpenNI2Grabber::startSynchronization () } catch (const IOException& exception) { - PCL_THROW_EXCEPTION (pcl::IOException, "Could not start synchronization " << exception.what ()); + std::cerr << exception.what() << std::endl; } } From ebd82de938627787078c57fe9ca9bd1458b3b365 Mon Sep 17 00:00:00 2001 From: Ky Waegel Date: Mon, 3 Mar 2014 17:14:34 -0500 Subject: [PATCH 07/12] Fixed data race problem with multiple sensors * The convertTo..PointCloud methods used statically allocated buffers to resize images, causing a data race if multiple deices tried to create point clouds simultaneously. Switched to using member buffers for each grabber. --- io/include/pcl/io/openni2_grabber.h | 9 ++- io/src/openni2_grabber.cpp | 95 ++++++++++++----------------- 2 files changed, 44 insertions(+), 60 deletions(-) diff --git a/io/include/pcl/io/openni2_grabber.h b/io/include/pcl/io/openni2_grabber.h index a41fbfaf7ea..2a113d5de4c 100644 --- a/io/include/pcl/io/openni2_grabber.h +++ b/io/include/pcl/io/openni2_grabber.h @@ -399,7 +399,7 @@ namespace pcl * \param[in] depth the depth image to convert */ boost::shared_ptr > - convertToXYZPointCloud (const pcl::io::openni2::DepthImage::Ptr &depth) const; + convertToXYZPointCloud (const pcl::io::openni2::DepthImage::Ptr &depth); /** \brief Convert a Depth + RGB image pair to a pcl::PointCloud * \param[in] image the RGB image to convert @@ -407,7 +407,7 @@ namespace pcl */ template typename pcl::PointCloud::Ptr convertToXYZRGBPointCloud (const pcl::io::openni2::Image::Ptr &image, - const pcl::io::openni2::DepthImage::Ptr &depth_image) const; + const pcl::io::openni2::DepthImage::Ptr &depth_image); /** \brief Convert a Depth + Intensity image pair to a pcl::PointCloud * \param[in] image the IR image to convert @@ -415,8 +415,11 @@ namespace pcl */ boost::shared_ptr > convertToXYZIPointCloud (const pcl::io::openni2::IRImage::Ptr &image, - const pcl::io::openni2::DepthImage::Ptr &depth_image) const; + const pcl::io::openni2::DepthImage::Ptr &depth_image); + std::vector color_resize_buffer_; + std::vector depth_resize_buffer_; + std::vector ir_resize_buffer_; // Stream callbacks ///////////////////////////////////////////////////// void diff --git a/io/src/openni2_grabber.cpp b/io/src/openni2_grabber.cpp index 94a235b1de6..043dae7b633 100644 --- a/io/src/openni2_grabber.cpp +++ b/io/src/openni2_grabber.cpp @@ -91,6 +91,9 @@ pcl::io::OpenNI2Grabber::OpenNI2Grabber (const std::string& device_id, const Mod , running_ (false) , rgb_parameters_(std::numeric_limits::quiet_NaN () ) , depth_parameters_(std::numeric_limits::quiet_NaN () ) + , color_resize_buffer_(0) + , depth_resize_buffer_(0) + , ir_resize_buffer_(0) { // initialize driver updateModeMaps (); // registering mapping from PCL enum modes to openni::VideoMode and vice versa @@ -509,7 +512,7 @@ pcl::io::OpenNI2Grabber::irDepthImageCallback (const IRImage::Ptr &ir_image, ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// pcl::PointCloud::Ptr -pcl::io::OpenNI2Grabber::convertToXYZPointCloud (const DepthImage::Ptr& depth_image) const +pcl::io::OpenNI2Grabber::convertToXYZPointCloud (const DepthImage::Ptr& depth_image) { pcl::PointCloud::Ptr cloud (new pcl::PointCloud ); @@ -545,20 +548,14 @@ pcl::io::OpenNI2Grabber::convertToXYZPointCloud (const DepthImage::Ptr& depth_im float bad_point = std::numeric_limits::quiet_NaN (); - // we have to use Data, since operator[] uses assert -> Debug-mode very slow! - register const unsigned short* depth_map = (const unsigned short*) depth_image->getData (); + const uint16_t* depth_map = (const uint16_t*) depth_image->getData (); if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_) { - static unsigned buffer_size = 0; - static boost::shared_array depth_buffer ((unsigned short*)(NULL)); + // Resize the image if nessacery + depth_resize_buffer_.resize(depth_width_ * depth_height_); - if (buffer_size < depth_width_ * depth_height_) - { - buffer_size = depth_width_ * depth_height_; - depth_buffer.reset (new unsigned short [buffer_size]); - } - depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ()); - depth_map = depth_buffer.get (); + depth_image->fillDepthImageRaw (depth_width_, depth_height_, (uint16_t*) depth_resize_buffer_.data() ); + depth_map = depth_resize_buffer_.data(); } register int depth_idx = 0; @@ -592,13 +589,8 @@ pcl::io::OpenNI2Grabber::convertToXYZPointCloud (const DepthImage::Ptr& depth_im ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template typename pcl::PointCloud::Ptr -pcl::io::OpenNI2Grabber::convertToXYZRGBPointCloud (const Image::Ptr &image, - const DepthImage::Ptr &depth_image) const +pcl::io::OpenNI2Grabber::convertToXYZRGBPointCloud (const Image::Ptr &image, const DepthImage::Ptr &depth_image) { - static unsigned rgb_array_size = 0; - static boost::shared_array rgb_array ((unsigned char*)(NULL)); - static unsigned char* rgb_buffer = 0; - boost::shared_ptr > cloud (new pcl::PointCloud); cloud->header.frame_id = rgb_frame_id_; @@ -631,30 +623,25 @@ pcl::io::OpenNI2Grabber::convertToXYZRGBPointCloud (const Image::Ptr &image, register float fx_inv = 1.0f / fx; register float fy_inv = 1.0f / fy; - register const OniDepthPixel* depth_map = (OniDepthPixel*) depth_image->getData (); + const uint16_t* depth_map = (const uint16_t*) depth_image->getData (); if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_) { - static unsigned buffer_size = 0; - static boost::shared_array depth_buffer ((unsigned short*)(NULL)); - - if (buffer_size < depth_width_ * depth_height_) - { - buffer_size = depth_width_ * depth_height_; - depth_buffer.reset (new unsigned short [buffer_size]); - } - - depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ()); - depth_map = depth_buffer.get (); + // Resize the image if nessacery + depth_resize_buffer_.resize(depth_width_ * depth_height_); + depth_map = depth_resize_buffer_.data(); + depth_image->fillDepthImageRaw (depth_width_, depth_height_, (unsigned short*) depth_map ); } - // here we need exact the size of the point cloud for a one-one correspondence! - if (rgb_array_size < image_width_ * image_height_ * 3) + const uint8_t* rgb_buffer = (const uint8_t*) image->getData (); + if (image->getWidth () != image_width_ || image->getHeight () != image_height_) { - rgb_array_size = image_width_ * image_height_ * 3; - rgb_array.reset (new unsigned char [rgb_array_size]); - rgb_buffer = rgb_array.get (); + // Resize the image if nessacery + color_resize_buffer_.resize(image_width_ * image_height_ * 3); + rgb_buffer = color_resize_buffer_.data(); + image->fillRGB (image_width_, image_height_, (unsigned char*) rgb_buffer, image_width_ * 3); } - image->fillRGB (image_width_, image_height_, rgb_buffer, image_width_ * 3); + + float bad_point = std::numeric_limits::quiet_NaN (); // set xyz to Nan and rgb to 0 (black) @@ -727,8 +714,7 @@ pcl::io::OpenNI2Grabber::convertToXYZRGBPointCloud (const Image::Ptr &image, ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// pcl::PointCloud::Ptr -pcl::io::OpenNI2Grabber::convertToXYZIPointCloud (const IRImage::Ptr &ir_image, - const DepthImage::Ptr &depth_image) const +pcl::io::OpenNI2Grabber::convertToXYZIPointCloud (const IRImage::Ptr &ir_image, const DepthImage::Ptr &depth_image) { boost::shared_ptr > cloud (new pcl::PointCloud ()); @@ -762,30 +748,25 @@ pcl::io::OpenNI2Grabber::convertToXYZIPointCloud (const IRImage::Ptr &ir_image, register float fy_inv = 1.0f / fy; - - register const unsigned short* depth_map = (const unsigned short*) depth_image->getData (); - register const unsigned short* ir_map = ir_image->getData (); - + const uint16_t* depth_map = (const uint16_t*) depth_image->getData (); if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_) { - static unsigned buffer_size = 0; - static boost::shared_array depth_buffer ((unsigned short*)(NULL)); - static boost::shared_array ir_buffer ((unsigned short*)(NULL)); - - if (buffer_size < depth_width_ * depth_height_) - { - buffer_size = depth_width_ * depth_height_; - depth_buffer.reset (new unsigned short [buffer_size]); - ir_buffer.reset (new unsigned short [buffer_size]); - } - - depth_image->fillDepthImageRaw (depth_width_, depth_height_, depth_buffer.get ()); - depth_map = depth_buffer.get (); + // Resize the image if nessacery + depth_resize_buffer_.resize(depth_width_ * depth_height_); + depth_map = depth_resize_buffer_.data(); + depth_image->fillDepthImageRaw (depth_width_, depth_height_, (unsigned short*) depth_map ); + } - ir_image->fillRaw (depth_width_, depth_height_, ir_buffer.get ()); - ir_map = ir_buffer.get (); + const uint16_t* ir_map = (const uint16_t*) ir_image->getData (); + if (ir_image->getWidth () != depth_width_ || ir_image->getHeight () != depth_height_) + { + // Resize the image if nessacery + ir_resize_buffer_.resize(depth_width_ * depth_height_); + ir_map = ir_resize_buffer_.data(); + ir_image->fillRaw (depth_width_, depth_height_, (unsigned short*) ir_map); } + register int depth_idx = 0; float bad_point = std::numeric_limits::quiet_NaN (); From c9c80e07502a93e3651f168fe53195aa7db52dfa Mon Sep 17 00:00:00 2001 From: Ky Waegel Date: Wed, 5 Mar 2014 19:13:40 -0500 Subject: [PATCH 08/12] Fixed warnings in OpenNI 2 grabberModeToOpenniMode * Fixed assignment in conditional test * Removed unused argument to focal length getters * Removed 'register' keywords that do nothing. --- io/include/pcl/io/openni2/openni2_device.h | 6 +-- io/src/openni2/openni2_device.cpp | 12 ++--- io/src/openni2_grabber.cpp | 63 +++++++++++----------- 3 files changed, 40 insertions(+), 41 deletions(-) diff --git a/io/include/pcl/io/openni2/openni2_device.h b/io/include/pcl/io/openni2/openni2_device.h index 6ffa8b1fb41..0574c9fa3b7 100644 --- a/io/include/pcl/io/openni2/openni2_device.h +++ b/io/include/pcl/io/openni2/openni2_device.h @@ -182,11 +182,11 @@ namespace pcl getDefaultDepthMode () const; float - getIRFocalLength (int output_y_resolution) const; + getIRFocalLength () const; float - getColorFocalLength (int output_y_resolution) const; + getColorFocalLength () const; float - getDepthFocalLength (int output_y_resolution) const; + getDepthFocalLength () const; // Baseline between sensors. Returns 0 if this value does not exist. float diff --git a/io/src/openni2/openni2_device.cpp b/io/src/openni2/openni2_device.cpp index 60b540d20ff..f7cdcbf6157 100644 --- a/io/src/openni2/openni2_device.cpp +++ b/io/src/openni2/openni2_device.cpp @@ -165,7 +165,7 @@ pcl::io::openni2::OpenNI2Device::isValid () const } float -pcl::io::openni2::OpenNI2Device::getIRFocalLength (int output_x_resolution) const +pcl::io::openni2::OpenNI2Device::getIRFocalLength () const { boost::shared_ptr stream = getIRVideoStream (); @@ -176,7 +176,7 @@ pcl::io::openni2::OpenNI2Device::getIRFocalLength (int output_x_resolution) cons } float -pcl::io::openni2::OpenNI2Device::getColorFocalLength (int output_x_resolution) const +pcl::io::openni2::OpenNI2Device::getColorFocalLength () const { boost::shared_ptr stream = getColorVideoStream (); @@ -187,7 +187,7 @@ pcl::io::openni2::OpenNI2Device::getColorFocalLength (int output_x_resolution) c } float -pcl::io::openni2::OpenNI2Device::getDepthFocalLength (int output_x_resolution) const +pcl::io::openni2::OpenNI2Device::getDepthFocalLength () const { boost::shared_ptr stream = getDepthVideoStream (); @@ -539,7 +539,7 @@ pcl::io::openni2::OpenNI2Device::getDefaultIRMode () const for (vector::iterator modeItr = modeList.begin (); modeItr != modeList.end (); modeItr++) { OpenNI2VideoMode mode = *modeItr; - if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ = 30.0) ) + if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ == 30.0) ) return mode; } return (modeList.at (0)); // Return first mode if we can't find VGA @@ -553,7 +553,7 @@ pcl::io::openni2::OpenNI2Device::getDefaultColorMode () const for (vector::iterator modeItr = modeList.begin (); modeItr != modeList.end (); modeItr++) { OpenNI2VideoMode mode = *modeItr; - if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ = 30.0) ) + if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ == 30.0) ) return mode; } return (modeList.at (0)); // Return first mode if we can't find VGA @@ -567,7 +567,7 @@ pcl::io::openni2::OpenNI2Device::getDefaultDepthMode () const for (vector::iterator modeItr = modeList.begin (); modeItr != modeList.end (); modeItr++) { OpenNI2VideoMode mode = *modeItr; - if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ = 30.0) ) + if ( (mode.x_resolution_ == 640) && (mode.y_resolution_ == 480) && (mode.frame_rate_ == 30.0) ) return mode; } return (modeList.at (0)); // Return first mode if we can't find VGA diff --git a/io/src/openni2_grabber.cpp b/io/src/openni2_grabber.cpp index 043dae7b633..ebfc49455fa 100644 --- a/io/src/openni2_grabber.cpp +++ b/io/src/openni2_grabber.cpp @@ -55,6 +55,7 @@ using namespace pcl::io::openni2; namespace pcl { + // Treat color as chars, float32, or uint32 typedef union { struct @@ -123,7 +124,7 @@ pcl::io::OpenNI2Grabber::OpenNI2Grabber (const std::string& device_id, const Mod rgb_sync_.addCallback (boost::bind (&OpenNI2Grabber::imageDepthImageCallback, this, _1, _2)); } - // Register callbacks from the sensor to the grabber + // callbacks from the sensor to the grabber device_->setColorCallback (boost::bind (&OpenNI2Grabber::processColorFrame, this, _1)); device_->setDepthCallback (boost::bind (&OpenNI2Grabber::processDepthFrame, this, _1)); device_->setIRCallback (boost::bind (&OpenNI2Grabber::processIRFrame, this, _1)); @@ -488,7 +489,7 @@ pcl::io::OpenNI2Grabber::imageDepthImageCallback (const Image::Ptr &image, if (image_depth_image_signal_->num_slots () > 0) { - float reciprocalFocalLength = 1.0f / device_->getDepthFocalLength (depth_width_); + float reciprocalFocalLength = 1.0f / device_->getDepthFocalLength (); image_depth_image_signal_->operator ()(image, depth_image, reciprocalFocalLength); } } @@ -504,7 +505,7 @@ pcl::io::OpenNI2Grabber::irDepthImageCallback (const IRImage::Ptr &ir_image, if (ir_depth_image_signal_->num_slots () > 0) { - float reciprocalFocalLength = 1.0f / device_->getDepthFocalLength (depth_width_); + float reciprocalFocalLength = 1.0f / device_->getDepthFocalLength (); ir_depth_image_signal_->operator ()(ir_image, depth_image, reciprocalFocalLength); } } @@ -522,10 +523,10 @@ pcl::io::OpenNI2Grabber::convertToXYZPointCloud (const DepthImage::Ptr& depth_im cloud->points.resize (cloud->height * cloud->width); - register float constant_x = 1.0f / device_->getDepthFocalLength (depth_width_); - register float constant_y = 1.0f / device_->getDepthFocalLength (depth_width_); - register float centerX = ((float)cloud->width - 1.f) / 2.f; - register float centerY = ((float)cloud->height - 1.f) / 2.f; + float constant_x = 1.0f / device_->getDepthFocalLength (); + float constant_y = 1.0f / device_->getDepthFocalLength (); + float centerX = ((float)cloud->width - 1.f) / 2.f; + float centerY = ((float)cloud->height - 1.f) / 2.f; if (pcl_isfinite (depth_parameters_.focal_length_x)) constant_x = 1.0f / static_cast (depth_parameters_.focal_length_x); @@ -539,8 +540,7 @@ pcl::io::OpenNI2Grabber::convertToXYZPointCloud (const DepthImage::Ptr& depth_im if (pcl_isfinite (depth_parameters_.principal_point_y)) centerY = static_cast (depth_parameters_.principal_point_y); - //if (device_->isDepthRegistered ()) - if (true) + if ( device_->isDepthRegistered() ) cloud->header.frame_id = rgb_frame_id_; else cloud->header.frame_id = depth_frame_id_; @@ -558,10 +558,10 @@ pcl::io::OpenNI2Grabber::convertToXYZPointCloud (const DepthImage::Ptr& depth_im depth_map = depth_resize_buffer_.data(); } - register int depth_idx = 0; + int depth_idx = 0; for (int v = 0; v < depth_height_; ++v) { - for (register int u = 0; u < depth_width_; ++u, ++depth_idx) + for (int u = 0; u < depth_width_; ++u, ++depth_idx) { pcl::PointXYZ& pt = cloud->points[depth_idx]; // Check for invalid measurements @@ -601,10 +601,10 @@ pcl::io::OpenNI2Grabber::convertToXYZRGBPointCloud (const Image::Ptr &image, con cloud->points.resize (cloud->height * cloud->width); // Generate default camera parameters - float fx = device_->getColorFocalLength (depth_width_); // Horizontal focal length - float fy = device_->getColorFocalLength (depth_width_); // Vertcal focal length - register float cx = ((float)cloud->width - 1.f) / 2.f; // Center x - register float cy = ((float)cloud->height - 1.f) / 2.f; // Center y + float fx = device_->getColorFocalLength (); // Horizontal focal length + float fy = device_->getColorFocalLength (); // Vertcal focal length + float cx = ((float)cloud->width - 1.f) / 2.f; // Center x + float cy = ((float)cloud->height - 1.f) / 2.f; // Center y // Load pre-calibrated camera parameters if they exist if (pcl_isfinite (depth_parameters_.focal_length_x)) @@ -620,8 +620,8 @@ pcl::io::OpenNI2Grabber::convertToXYZRGBPointCloud (const Image::Ptr &image, con cy = static_cast (depth_parameters_.principal_point_y); // Get inverse focal length for calculations below - register float fx_inv = 1.0f / fx; - register float fy_inv = 1.0f / fy; + float fx_inv = 1.0f / fx; + float fy_inv = 1.0f / fy; const uint16_t* depth_map = (const uint16_t*) depth_image->getData (); if (depth_image->getWidth () != depth_width_ || depth_image->getHeight () != depth_height_) @@ -662,7 +662,7 @@ pcl::io::OpenNI2Grabber::convertToXYZRGBPointCloud (const Image::Ptr &image, con int point_idx = 0; for (int v = 0; v < depth_height_; ++v, point_idx += skip) { - for (register int u = 0; u < depth_width_; ++u, ++value_idx, point_idx += step) + for (int u = 0; u < depth_width_; ++u, ++value_idx, point_idx += step) { PointT& pt = cloud->points[point_idx]; /// @todo Different values for these cases @@ -726,17 +726,17 @@ pcl::io::OpenNI2Grabber::convertToXYZIPointCloud (const IRImage::Ptr &ir_image, cloud->points.resize (cloud->height * cloud->width); - float fx = device_->getColorFocalLength (depth_width_); // Horizontal focal length - float fy = device_->getColorFocalLength (depth_width_); // Vertcal focal length - register float cx = ((float)cloud->width - 1.f) / 2.f; // Center x - register float cy = ((float)cloud->height - 1.f) / 2.f; // Center y + float fx = device_->getColorFocalLength (); // Horizontal focal length + float fy = device_->getColorFocalLength (); // Vertcal focal length + float cx = ((float)cloud->width - 1.f) / 2.f; // Center x + float cy = ((float)cloud->height - 1.f) / 2.f; // Center y // Load pre-calibrated camera parameters if they exist if (pcl_isfinite (depth_parameters_.focal_length_x)) - fx = 1.0f / static_cast (depth_parameters_.focal_length_x); + fx = static_cast (depth_parameters_.focal_length_x); if (pcl_isfinite (depth_parameters_.focal_length_y)) - fy = 1.0f / static_cast (depth_parameters_.focal_length_y); + fy = static_cast (depth_parameters_.focal_length_y); if (pcl_isfinite (depth_parameters_.principal_point_x)) cx = static_cast (depth_parameters_.principal_point_x); @@ -744,8 +744,8 @@ pcl::io::OpenNI2Grabber::convertToXYZIPointCloud (const IRImage::Ptr &ir_image, if (pcl_isfinite (depth_parameters_.principal_point_y)) cy = static_cast (depth_parameters_.principal_point_y); - register float fx_inv = 1.0f / fx; - register float fy_inv = 1.0f / fy; + float fx_inv = 1.0f / fx; + float fy_inv = 1.0f / fy; const uint16_t* depth_map = (const uint16_t*) depth_image->getData (); @@ -767,12 +767,12 @@ pcl::io::OpenNI2Grabber::convertToXYZIPointCloud (const IRImage::Ptr &ir_image, } - register int depth_idx = 0; + int depth_idx = 0; float bad_point = std::numeric_limits::quiet_NaN (); for (int v = 0; v < depth_height_; ++v) { - for (register int u = 0; u < depth_width_; ++u, ++depth_idx) + for (int u = 0; u < depth_width_; ++u, ++depth_idx) { pcl::PointXYZI& pt = cloud->points[depth_idx]; /// @todo Different values for these cases @@ -786,8 +786,8 @@ pcl::io::OpenNI2Grabber::convertToXYZIPointCloud (const IRImage::Ptr &ir_image, else { pt.z = depth_map[depth_idx] * 0.001f; // millimeters to meters - pt.x = (static_cast (u) - cx) * pt.z * fx; - pt.y = (static_cast (v) - cy) * pt.z * fy; + pt.x = (static_cast (u) - cx) * pt.z * fx_inv; + pt.y = (static_cast (v) - cy) * pt.z * fy_inv; } pt.data_c[0] = pt.data_c[1] = pt.data_c[2] = pt.data_c[3] = 0; @@ -905,8 +905,7 @@ void pcl::io::OpenNI2Grabber::processDepthFrame (openni::VideoStream& stream) FrameWrapper::Ptr frameWrapper = boost::make_shared(frame); boost::posix_time::ptime t_readFrameTimestamp = boost::posix_time::microsec_clock::local_time (); - int frameWidth = frame.getWidth (); - float focalLength = device_->getDepthFocalLength (frameWidth); + float focalLength = device_->getDepthFocalLength (); float baseline = device_->getBaseline(); uint64_t no_sample_value = device_->getShadowValue(); From 2a06fc1610c90899db5f35362ac3b9ae828bedb0 Mon Sep 17 00:00:00 2001 From: Ky Waegel Date: Wed, 25 Sep 2013 01:39:38 -0400 Subject: [PATCH 09/12] Added OpenNI 2 viewer app to visualization module. * Changed default viewpoint for OpenNI 2 viewer app. Needed to look down +Z axis to see point cloud. --- visualization/CMakeLists.txt | 8 +- visualization/tools/CMakeLists.txt | 4 + visualization/tools/openni2_viewer.cpp | 370 +++++++++++++++++++++++++ 3 files changed, 381 insertions(+), 1 deletion(-) create mode 100644 visualization/tools/openni2_viewer.cpp diff --git a/visualization/CMakeLists.txt b/visualization/CMakeLists.txt index 388fbb63a35..65bb3b6ab0e 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) +PCL_SUBSYS_DEPEND(build "${SUBSYS_NAME}" DEPS ${SUBSYS_DEPS} EXT_DEPS vtk OPT_DEPS openni openni2) if (ANDROID) set (build FALSE) @@ -153,6 +153,12 @@ if(build) endif() set(EXT_DEPS "") + if(OPENNI_FOUND) + list(APPEND EXT_DEPS openni-dev) + endif(OPENNI_FOUND) + if(OPENNI2_FOUND) + list(APPEND EXT_DEPS openni2-dev) + endif(OPENNI2_FOUND) 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 422d9e53b35..ec48ac71168 100644 --- a/visualization/tools/CMakeLists.txt +++ b/visualization/tools/CMakeLists.txt @@ -34,3 +34,7 @@ if(OPENNI_FOUND AND BUILD_OPENNI) target_link_libraries(pcl_openni_image pcl_common pcl_io pcl_kdtree pcl_visualization) endif() +if(OPENNI2_FOUND AND BUILD_OPENNI2) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_openni2_viewer ${SUBSYS_NAME} openni2_viewer.cpp) + target_link_libraries(pcl_openni2_viewer pcl_common pcl_io pcl_kdtree pcl_visualization) +endif() diff --git a/visualization/tools/openni2_viewer.cpp b/visualization/tools/openni2_viewer.cpp new file mode 100644 index 00000000000..595e5a9c14c --- /dev/null +++ b/visualization/tools/openni2_viewer.cpp @@ -0,0 +1,370 @@ +/* +* Software License Agreement (BSD License) +* +* Copyright (c) 2010, Willow Garage, 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 Willow Garage, Inc. 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. +* +* $Id$ +* +*/ + +#define MEASURE_FUNCTION_TIME +#include //fps calculations +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "pcl/io/openni2/openni.h" + +typedef boost::chrono::high_resolution_clock HRClock; + +#define SHOW_FPS 1 +#if SHOW_FPS +#define FPS_CALC(_WHAT_) \ + do \ +{ \ + static unsigned count = 0;\ + static double last = pcl::getTime ();\ + double now = pcl::getTime (); \ + ++count; \ + if (now - last >= 1.0) \ +{ \ + std::cout << "Average framerate ("<< _WHAT_ << "): " << double (count)/double (now - last) << " Hz" << std::endl; \ + count = 0; \ + last = now; \ +} \ +}while (false) +#else +#define FPS_CALC (_WHAT_) \ + do \ +{ \ +}while (false) +#endif + +void +printHelp (int, char **argv) +{ + using pcl::console::print_error; + using pcl::console::print_info; + + print_error ("Syntax is: %s [(( | ) [-depthmode ] [-imagemode ] [-xyz] | -l []| -h | --help)]\n", argv [0]); + print_info ("%s -h | --help : shows this help\n", argv [0]); + print_info ("%s -xyz : use only XYZ values and ignore RGB components (this flag is required for use with ASUS Xtion Pro) \n", argv [0]); + print_info ("%s -l : list all available devices\n", argv [0]); + print_info ("%s -l :list all available modes for specified device\n", argv [0]); + print_info ("\t\t may be \"#1\", \"#2\", ... for the first, second etc device in the list\n"); +#ifndef _WIN32 + print_info ("\t\t bus@address for the device connected to a specific usb-bus / address combination\n"); + print_info ("\t\t \n"); +#endif + print_info ("\n\nexamples:\n"); + print_info ("%s \"#1\"\n", argv [0]); + print_info ("\t\t uses the first device.\n"); + print_info ("%s \"./temp/test.oni\"\n", argv [0]); + print_info ("\t\t uses the oni-player device to play back oni file given by path.\n"); + print_info ("%s -l\n", argv [0]); + print_info ("\t\t list all available devices.\n"); + print_info ("%s -l \"#2\"\n", argv [0]); + print_info ("\t\t list all available modes for the second device.\n"); +#ifndef _WIN32 + print_info ("%s A00361800903049A\n", argv [0]); + print_info ("\t\t uses the device with the serial number \'A00361800903049A\'.\n"); + print_info ("%s 1@16\n", argv [0]); + print_info ("\t\t uses the device on address 16 at USB bus 1.\n"); +#endif +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template +class OpenNI2Viewer +{ +public: + typedef pcl::PointCloud Cloud; + typedef typename Cloud::ConstPtr CloudConstPtr; + + OpenNI2Viewer (pcl::io::OpenNI2Grabber& grabber) + : cloud_viewer_ (new pcl::visualization::PCLVisualizer ("PCL OpenNI2 cloud")) + , image_viewer_ () + , grabber_ (grabber) + , rgb_data_ (0), rgb_data_size_ (0) + { + } + + void + cloud_callback (const CloudConstPtr& cloud) + { + FPS_CALC ("cloud callback"); + boost::mutex::scoped_lock lock (cloud_mutex_); + cloud_ = cloud; + } + + void + image_callback (const boost::shared_ptr& image) + { + FPS_CALC ("image callback"); + boost::mutex::scoped_lock lock (image_mutex_); + image_ = image; + + if (image->getEncoding () != pcl::io::openni2::Image::RGB) + { + if (rgb_data_size_ < image->getWidth () * image->getHeight ()) + { + if (rgb_data_) + delete [] rgb_data_; + rgb_data_size_ = image->getWidth () * image->getHeight (); + rgb_data_ = new unsigned char [rgb_data_size_ * 3]; + } + image_->fillRGB (image_->getWidth (), image_->getHeight (), rgb_data_); + } + } + + void + keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*) + { + if (event.getKeyCode ()) + cout << "the key \'" << event.getKeyCode () << "\' (" << event.getKeyCode () << ") was"; + else + cout << "the special key \'" << event.getKeySym () << "\' was"; + if (event.keyDown ()) + cout << " pressed" << endl; + else + cout << " released" << endl; + } + + void + mouse_callback (const pcl::visualization::MouseEvent& mouse_event, void*) + { + if (mouse_event.getType () == pcl::visualization::MouseEvent::MouseButtonPress && mouse_event.getButton () == pcl::visualization::MouseEvent::LeftButton) + { + cout << "left button pressed @ " << mouse_event.getX () << " , " << mouse_event.getY () << endl; + } + } + + /** + * @brief starts the main loop + */ + void + run () + { + cloud_viewer_->registerMouseCallback (&OpenNI2Viewer::mouse_callback, *this); + cloud_viewer_->registerKeyboardCallback (&OpenNI2Viewer::keyboard_callback, *this); + cloud_viewer_->setCameraFieldOfView (1.02259994f); + boost::function cloud_cb = boost::bind (&OpenNI2Viewer::cloud_callback, this, _1); + boost::signals2::connection cloud_connection = grabber_.registerCallback (cloud_cb); + + boost::signals2::connection image_connection; + if (grabber_.providesCallback&)>()) + { + image_viewer_.reset (new pcl::visualization::ImageViewer ("PCL OpenNI image")); + image_viewer_->registerMouseCallback (&OpenNI2Viewer::mouse_callback, *this); + image_viewer_->registerKeyboardCallback (&OpenNI2Viewer::keyboard_callback, *this); + boost::function&) > image_cb = boost::bind (&OpenNI2Viewer::image_callback, this, _1); + image_connection = grabber_.registerCallback (image_cb); + } + + bool image_init = false, cloud_init = false; + + grabber_.start (); + + while (!cloud_viewer_->wasStopped () && (image_viewer_ && !image_viewer_->wasStopped ())) + { + boost::shared_ptr image; + CloudConstPtr cloud; + + cloud_viewer_->spinOnce (); + + // See if we can get a cloud + if (cloud_mutex_.try_lock ()) + { + cloud_.swap (cloud); + cloud_mutex_.unlock (); + } + + if (cloud) + { + FPS_CALC("drawing cloud"); + + if (!cloud_init) + { + cloud_viewer_->setPosition (0, 0); + cloud_viewer_->setSize (cloud->width, cloud->height); + cloud_init = !cloud_init; + } + + if (!cloud_viewer_->updatePointCloud (cloud, "OpenNICloud")) + { + cloud_viewer_->addPointCloud (cloud, "OpenNICloud"); + cloud_viewer_->resetCameraViewpoint ("OpenNICloud"); + cloud_viewer_->setCameraPosition ( + 0,0,0, // Position + 0,0,1, // Viewpoint + 0,-1,0); // Up + } + } + + // See if we can get an image + if (image_mutex_.try_lock ()) + { + image_.swap (image); + image_mutex_.unlock (); + } + + + if (image) + { + if (!image_init && cloud && cloud->width != 0) + { + image_viewer_->setPosition (cloud->width, 0); + image_viewer_->setSize (cloud->width, cloud->height); + image_init = !image_init; + } + + if (image->getEncoding () == pcl::io::openni2::Image::RGB) + image_viewer_->addRGBImage ( (const unsigned char*)image->getData (), image->getWidth (), image->getHeight ()); + else + image_viewer_->addRGBImage (rgb_data_, image->getWidth (), image->getHeight ()); + image_viewer_->spinOnce (); + + } + } + + grabber_.stop (); + + cloud_connection.disconnect (); + image_connection.disconnect (); + if (rgb_data_) + delete[] rgb_data_; + } + + boost::shared_ptr cloud_viewer_; + boost::shared_ptr image_viewer_; + + pcl::io::OpenNI2Grabber& grabber_; + boost::mutex cloud_mutex_; + boost::mutex image_mutex_; + + CloudConstPtr cloud_; + boost::shared_ptr image_; + unsigned char* rgb_data_; + unsigned rgb_data_size_; +}; + +// Create the PCLVisualizer object +boost::shared_ptr cld; +boost::shared_ptr img; + +/* ---[ */ +int +main (int argc, char** argv) +{ + std::string device_id (""); + pcl::io::OpenNI2Grabber::Mode depth_mode = pcl::io::OpenNI2Grabber::OpenNI_Default_Mode; + pcl::io::OpenNI2Grabber::Mode image_mode = pcl::io::OpenNI2Grabber::OpenNI_Default_Mode; + bool xyz = false; + + if (argc >= 2) + { + device_id = argv[1]; + if (device_id == "--help" || device_id == "-h") + { + printHelp (argc, argv); + return 0; + } + else if (device_id == "-l") + { + if (argc >= 3) + { + pcl::io::OpenNI2Grabber grabber (argv[2]); + boost::shared_ptr device = grabber.getDevice (); + cout << *device; // Prints out all sensor data, including supported video modes + } + else + { + boost::shared_ptr deviceManager = pcl::io::openni2::OpenNI2DeviceManager::getInstance (); + if (deviceManager->getNumOfConnectedDevices () > 0) + { + for (unsigned deviceIdx = 0; deviceIdx < deviceManager->getNumOfConnectedDevices (); ++deviceIdx) + { + boost::shared_ptr device = deviceManager->getDeviceByIndex (deviceIdx); + cout << "Device " << device->getStringID () << "connected." << endl; + } + + } + else + cout << "No devices connected." << endl; + + cout <<"Virtual Devices available: ONI player" << endl; + } + return 0; + } + } + else + { + boost::shared_ptr deviceManager = pcl::io::openni2::OpenNI2DeviceManager::getInstance (); + if (deviceManager->getNumOfConnectedDevices () > 0) + { + boost::shared_ptr device = deviceManager->getAnyDevice (); + cout << "Device ID not set, using default device: " << device->getStringID () << endl; + } + } + + unsigned mode; + if (pcl::console::parse (argc, argv, "-depthmode", mode) != -1) + depth_mode = pcl::io::OpenNI2Grabber::Mode (mode); + + if (pcl::console::parse (argc, argv, "-imagemode", mode) != -1) + image_mode = pcl::io::OpenNI2Grabber::Mode (mode); + + if (pcl::console::find_argument (argc, argv, "-xyz") != -1) + xyz = true; + + pcl::io::OpenNI2Grabber grabber (device_id, depth_mode, image_mode); + + if (xyz || !grabber.providesCallback ()) + { + OpenNI2Viewer openni_viewer (grabber); + openni_viewer.run (); + } + else + { + OpenNI2Viewer openni_viewer (grabber); + openni_viewer.run (); + } + + return (0); +} +/* ]--- */ From 24a24c9f4c519b102e2dfb2f3e106a8b45457861 Mon Sep 17 00:00:00 2001 From: Ky Waegel Date: Thu, 27 Feb 2014 18:17:18 -0500 Subject: [PATCH 10/12] Added HAVE_OPENNI check to people_app * Depends on pcl/io/openni_grabber.h and does not work with OpenNI 2.x grabber yet --- gpu/people/tools/CMakeLists.txt | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/gpu/people/tools/CMakeLists.txt b/gpu/people/tools/CMakeLists.txt index 07f334d289d..e3d18a8aaad 100644 --- a/gpu/people/tools/CMakeLists.txt +++ b/gpu/people/tools/CMakeLists.txt @@ -16,8 +16,10 @@ include_directories(${VTK_INCLUDE_DIRS}) #PCL_ADD_EXECUTABLE("${the_target}" "${SUBSYS_NAME}" people_tracking.cpp) #target_link_libraries("${the_target}" pcl_common pcl_filters pcl_kdtree pcl_segmentation pcl_kdtree pcl_gpu_people pcl_filters pcl_io pcl_visualization) -PCL_ADD_EXECUTABLE(pcl_people_app "${SUBSYS_NAME}" people_app.cpp) -target_link_libraries (pcl_people_app pcl_common pcl_filters pcl_kdtree pcl_segmentation pcl_kdtree pcl_gpu_people pcl_filters pcl_io pcl_visualization ${Boost_LIBRARIES}) +if (HAVE_OPENNI) + PCL_ADD_EXECUTABLE(pcl_people_app "${SUBSYS_NAME}" people_app.cpp) + target_link_libraries (pcl_people_app pcl_common pcl_filters pcl_kdtree pcl_segmentation pcl_kdtree pcl_gpu_people pcl_filters pcl_io pcl_visualization ${Boost_LIBRARIES}) +endif (HAVE_OPENNI) PCL_ADD_EXECUTABLE(pcl_people_pcd_prob "${SUBSYS_NAME}" people_pcd_prob.cpp) target_link_libraries (pcl_people_pcd_prob pcl_common pcl_filters pcl_kdtree pcl_segmentation pcl_kdtree pcl_gpu_people pcl_filters pcl_io pcl_visualization ${Boost_LIBRARIES}) From 9a89c2982a314f5b487d3ce40f4a3fe989c9b8e7 Mon Sep 17 00:00:00 2001 From: Vadim Frolov Date: Thu, 6 Mar 2014 10:27:14 +0100 Subject: [PATCH 11/12] Move common image files to pcl/io/ during the installation; fix block alignments. --- io/CMakeLists.txt | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/io/CMakeLists.txt b/io/CMakeLists.txt index 3d0fd82c227..0dea55588a5 100644 --- a/io/CMakeLists.txt +++ b/io/CMakeLists.txt @@ -53,7 +53,6 @@ if(build) include/pcl/io/openni2/openni2_device.h include/pcl/io/openni2/openni2_device_info.h include/pcl/io/openni2/openni2_device_manager.h - ${IMAGE_INCLUDES} ) set(OPENNI2_GRABBER_SOURCES @@ -64,11 +63,10 @@ if(build) src/openni2/openni2_device.cpp src/openni2/openni2_device_info.cpp src/openni2/openni2_device_manager.cpp - ${IMAGE_SOURCES} ) - source_group("OpenNI 2\\Header Files" FILES ${OPENNI2_GRABBER_INCLUDES} ${OPENNI2_INCLUDES}) - source_group("OpenNI 2\\Source Files" FILES ${OPENNI2_GRABBER_SOURCES}) + source_group("OpenNI 2\\Header Files" FILES ${OPENNI2_GRABBER_INCLUDES} ${OPENNI2_INCLUDES} ${IMAGE_INCLUDES}) + source_group("OpenNI 2\\Source Files" FILES ${OPENNI2_GRABBER_SOURCES} ${IMAGE_SOURCES}) # Copy OpenNI2 redist directory to bin. Needed for driver modules. Only tested on Windows. if(MSVC) @@ -207,6 +205,7 @@ if(build) ${VTK_IO_SOURCE} ${OPENNI_GRABBER_SOURCES} ${OPENNI2_GRABBER_SOURCES} + ${IMAGE_SOURCES} ${DINAST_GRABBER_SOURCES} ${FZAPI_GRABBER_SOURCES} ${PXC_GRABBER_SOURCES} @@ -249,6 +248,7 @@ if(build) ${VTK_IO_INCLUDES} ${OPENNI_GRABBER_INCLUDES} ${OPENNI2_GRABBER_INCLUDES} + ${IMAGE_INCLUDES} ${DINAST_GRABBER_INCLUDES} ${FZAPI_GRABBER_INCLUDES} ${PXC_GRABBER_INCLUDES} @@ -343,7 +343,7 @@ if(build) PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}" ${incs}) PCL_ADD_INCLUDES("${SUBSYS_NAME}" compression ${compression_incs}) PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/openni_camera" ${OPENNI_INCLUDES}) - PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/openni2_camera" ${OPENNI2_INCLUDES}) + PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/openni2" ${OPENNI2_INCLUDES}) PCL_ADD_INCLUDES("${SUBSYS_NAME}" "${SUBSYS_NAME}/impl" ${impl_incs}) add_subdirectory(tools) From 0a7a0f3b9787f3cdcf048867e9f4418a24941bc6 Mon Sep 17 00:00:00 2001 From: Ky Waegel Date: Fri, 2 May 2014 04:46:19 -0400 Subject: [PATCH 12/12] Added common search path for deb packages. * /usr/include/openni2 is a common install location for unofficial OpenNI2 deb packages. --- cmake/Modules/FindOpenNI2.cmake | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/cmake/Modules/FindOpenNI2.cmake b/cmake/Modules/FindOpenNI2.cmake index a33fa343aca..036e4e0423b 100644 --- a/cmake/Modules/FindOpenNI2.cmake +++ b/cmake/Modules/FindOpenNI2.cmake @@ -48,8 +48,10 @@ if(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) endif(WIN32 AND CMAKE_SIZEOF_VOID_P EQUAL 8) find_path(OPENNI2_INCLUDE_DIRS OpenNI.h - PATHS "$ENV{OPENNI2_INCLUDE${OPENNI2_SUFFIX}}" # Linux/Windows default path, Win64 needs '64' suffix - ) + PATHS + "$ENV{OPENNI2_INCLUDE${OPENNI2_SUFFIX}}" # Win64 needs '64' suffix + /usr/include/openni2 # common path for deb packages +) find_library(OPENNI2_LIBRARY NAMES OpenNI2 # No suffix needed on Win64