From bc2da10713b9123083f1170fdf4fd21fb53ce2b7 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 21 Apr 2017 21:24:23 -0400 Subject: [PATCH] RealSense driver: added odometry option (ZR300) --- CMakeLists.txt | 11 + Version.h.in | 1 + cmake_modules/FindRealSense.cmake | 27 ++ corelib/include/rtabmap/core/CameraRGBD.h | 16 + corelib/src/CameraRGBD.cpp | 415 +++++++++++++++++++++- examples/RGBDMapping/main.cpp | 2 +- guilib/src/PreferencesDialog.cpp | 10 + guilib/src/ui/preferencesDialog.ui | 57 ++- 8 files changed, 509 insertions(+), 30 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0c19bcbf..1a096bb8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -319,6 +319,9 @@ IF(WITH_REALSENSE) IF(RealSense_FOUND) MESSAGE(STATUS "Found RealSense: ${RealSense_INCLUDE_DIRS}") ENDIF(RealSense_FOUND) + IF(RealSenseSlam_FOUND) + MESSAGE(STATUS "Found RealSenseSlam: ${RealSense_INCLUDE_DIRS}") + ENDIF(RealSenseSlam_FOUND) ENDIF(WITH_REALSENSE) IF(WITH_OCTOMAP) @@ -435,6 +438,9 @@ ENDIF() IF(NOT RealSense_FOUND) SET(REALSENSE "//") ELSE() + IF(NOT RealSenseSlam_FOUND) + SET(REALSENSESLAM "//") + ENDIF(NOT RealSenseSlam_FOUND) SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${RealSense_LIBRARIES}) ENDIF() IF(NOT OCTOMAP_FOUND) @@ -736,6 +742,11 @@ ENDIF() IF(RealSense_FOUND) MESSAGE(STATUS " With RealSense = YES (License: Apache-2)") +IF(RealSenseSlam_FOUND) +MESSAGE(STATUS " With RealSenseSlam = YES") +ELSE() +MESSAGE(STATUS " With RealSenseSlam = NO (librealsense_slam not found)") +ENDIF() ELSEIF(NOT WITH_REALSENSE) MESSAGE(STATUS " With RealSense = NO (WITH_REALSENSE=OFF)") ELSE() diff --git a/Version.h.in b/Version.h.in index 93343a25..2b918fe8 100644 --- a/Version.h.in +++ b/Version.h.in @@ -51,6 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2 @ZED@#define RTABMAP_ZED @REALSENSE@#define RTABMAP_REALSENSE +@REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM @OCTOMAP@#define RTABMAP_OCTOMAP #endif /* VERSION_H_ */ diff --git a/cmake_modules/FindRealSense.cmake b/cmake_modules/FindRealSense.cmake index 87377508..734fdf78 100644 --- a/cmake_modules/FindRealSense.cmake +++ b/cmake_modules/FindRealSense.cmake @@ -4,6 +4,7 @@ # # It sets the following variables: # RealSense_FOUND - Set to false, or undefined, if RealSense isn't found. +# RealSenseSlam_FOUND - Set to false, or undefined, if RealSense slam module isn't found. # RealSense_INCLUDE_DIRS - The RealSense include directory. # RealSense_LIBRARIES - The RealSense library to link against. @@ -19,9 +20,35 @@ IF (RealSense_INCLUDE_DIRS AND RealSense_LIBRARY) SET(RealSense_FOUND TRUE) ENDIF (RealSense_INCLUDE_DIRS AND RealSense_LIBRARY) +#SLAM +find_path(RealSenseSlam_INCLUDE_DIRS NAMES librealsense/slam/slam.h PATHS $ENV{RealSense_ROOT_DIR}/include) +if(CMAKE_CL_64) +find_library(RealSenseSlam_LIBRARY NAMES realsense_slam PATHS $ENV{RealSense_ROOT_DIR}/lib $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64) +find_library(RealSenseImage_LIBRARY NAMES realsense_image PATHS $ENV{RealSense_ROOT_DIR}/lib $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64) +find_library(RealSenseSP_Core_LIBRARY NAMES SP_Core PATHS $ENV{RealSense_ROOT_DIR}/lib $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64) +find_library(RealSenseTracker_LIBRARY NAMES tracker PATHS $ENV{RealSense_ROOT_DIR}/lib $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/x64) +else() +find_library(RealSenseSlam_LIBRARY NAMES realsense_slam PATHS $ENV{RealSense_ROOT_DIR}/lib $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32) +find_library(RealSenseImage_LIBRARY NAMES realsense_image PATHS $ENV{RealSense_ROOT_DIR}/lib $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32) +find_library(RealSenseSP_Core_LIBRARY NAMES SP_Core PATHS $ENV{RealSense_ROOT_DIR}/lib $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32) +find_library(RealSenseTracker_LIBRARY NAMES tracker PATHS $ENV{RealSense_ROOT_DIR}/lib $ENV{RealSense_ROOT_DIR}/bin $ENV{RealSense_ROOT_DIR}/bin/Win32) +endif() + +IF (RealSenseSlam_INCLUDE_DIRS AND RealSenseSlam_LIBRARY AND RealSenseImage_LIBRARY AND RealSenseSP_Core_LIBRARY AND RealSenseTracker_LIBRARY) + SET(RealSenseSlam_FOUND TRUE) +ENDIF (RealSenseSlam_INCLUDE_DIRS AND RealSenseSlam_LIBRARY AND RealSenseImage_LIBRARY AND RealSenseSP_Core_LIBRARY AND RealSenseTracker_LIBRARY) + IF (RealSense_FOUND) # show which RealSense was found only if not quiet SET(RealSense_LIBRARIES ${RealSense_LIBRARY}) + IF (RealSenseSlam_FOUND) + SET(RealSense_LIBRARIES + ${RealSense_LIBRARIES} + ${RealSenseSlam_LIBRARY} + ${RealSenseImage_LIBRARY} + ${RealSenseSP_Core_LIBRARY} + ${RealSenseTracker_LIBRARY}) + ENDIF(RealSenseSlam_FOUND) IF (NOT RealSense_FIND_QUIETLY) MESSAGE(STATUS "Found RealSense: ${RealSense_LIBRARIES}") ENDIF (NOT RealSense_FIND_QUIETLY) diff --git a/corelib/include/rtabmap/core/CameraRGBD.h b/corelib/include/rtabmap/core/CameraRGBD.h index 4c1c5385..3f7b6638 100644 --- a/corelib/include/rtabmap/core/CameraRGBD.h +++ b/corelib/include/rtabmap/core/CameraRGBD.h @@ -69,6 +69,9 @@ namespace rs { class context; class device; + namespace slam { + class slam; + } } typedef struct _freenect_context freenect_context; @@ -290,6 +293,7 @@ private: ///////////////////////// // CameraRealSense ///////////////////////// +class slam_event_handler; class RTABMAP_EXP CameraRealSense : public Camera { @@ -302,6 +306,7 @@ public: int deviceId = 0, int presetRGB = 0, // 0=best quality, 1=largest image, 2=highest framerate int presetDepth = 0, // 0=best quality, 1=largest image, 2=highest framerate + bool computeOdometry = false, float imageRate = 0, const Transform & localTransform = Transform::getIdentity()); virtual ~CameraRealSense(); @@ -309,6 +314,7 @@ public: virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); virtual bool isCalibrated() const; virtual std::string getSerial() const; + virtual bool odomProvided() const; protected: virtual SensorData captureImage(CameraInfo * info = 0); @@ -320,6 +326,16 @@ private: int deviceId_; int presetRGB_; int presetDepth_; + bool computeOdometry_; + + int motionSeq_[2]; + rs::slam::slam * slam_; + UMutex slamLock_; + + std::map > bufferedFrames_; + std::pair lastSyncFrames_; + UMutex dataMutex_; + USemaphore dataReady_; #endif }; diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp index 9c928c8c..3d960fcc 100644 --- a/corelib/src/CameraRGBD.cpp +++ b/corelib/src/CameraRGBD.cpp @@ -66,6 +66,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifdef RTABMAP_REALSENSE #include +#include +#include +#ifdef RTABMAP_REALSENSE_SLAM +#include +#endif #endif #ifdef RTABMAP_OPENNI2 @@ -1974,7 +1979,13 @@ bool CameraRealSense::available() #endif } -CameraRealSense::CameraRealSense(int device, int presetRGB, int presetDepth, float imageRate, const rtabmap::Transform & localTransform) : +CameraRealSense::CameraRealSense( + int device, + int presetRGB, + int presetDepth, + bool computeOdometry, + float imageRate, + const rtabmap::Transform & localTransform) : Camera(imageRate, localTransform) #ifdef RTABMAP_REALSENSE , @@ -1982,7 +1993,9 @@ CameraRealSense::CameraRealSense(int device, int presetRGB, int presetDepth, flo dev_(0), deviceId_(device), presetRGB_(presetRGB), - presetDepth_(presetDepth) + presetDepth_(presetDepth), + computeOdometry_(computeOdometry), + slam_(0) #endif { UDEBUG(""); @@ -1990,18 +2003,89 @@ CameraRealSense::CameraRealSense(int device, int presetRGB, int presetDepth, flo CameraRealSense::~CameraRealSense() { + UDEBUG(""); #ifdef RTABMAP_REALSENSE + UDEBUG(""); + if(dev_) + { + if(slam_!=0) + { + dev_->stop(rs::source::all_sources); + } + else + { + dev_->stop(); + } + dev_ = 0; + } + UDEBUG(""); if (ctx_) { delete ctx_; } +#ifdef RTABMAP_REALSENSE_SLAM + UDEBUG(""); + if(slam_) + { + UScopeMutex lock(slamLock_); + slam_->flush_resources(); + delete slam_; + slam_ = 0; + } +#endif #endif } +#ifdef RTABMAP_REALSENSE_SLAM +bool setStreamConfigIntrin( + rs::core::stream_type stream, + std::map< rs::core::stream_type, rs::core::intrinsics > intrinsics, + rs::core::video_module_interface::supported_module_config & supported_config, + rs::core::video_module_interface::actual_module_config & actual_config) +{ + auto & supported_stream_config = supported_config[stream]; + if (!supported_stream_config.is_enabled || supported_stream_config.size.width != intrinsics[stream].width || supported_stream_config.size.height != intrinsics[stream].height) + { + UERROR("size of stream is not supported by slam"); + UERROR(" supported: stream %d, width: %d height: %d", (uint32_t) stream, supported_stream_config.size.width, supported_stream_config.size.height); + UERROR(" received: stream %d, width: %d height: %d", (uint32_t) stream, intrinsics[stream].width, intrinsics[stream].height); + + return false; + } + rs::core::video_module_interface::actual_image_stream_config &actual_stream_config = actual_config[stream]; + actual_config[stream].size.width = intrinsics[stream].width; + actual_config[stream].size.height = intrinsics[stream].height; + actual_stream_config.frame_rate = supported_stream_config.frame_rate; + actual_stream_config.intrinsics = intrinsics[stream]; + actual_stream_config.is_enabled = true; + return true; +} +#endif + bool CameraRealSense::init(const std::string & calibrationFolder, const std::string & cameraName) { UDEBUG(""); #ifdef RTABMAP_REALSENSE + + if(dev_) + { + dev_->stop(rs::source::all_sources); + dev_ = 0; + } + bufferedFrames_.clear(); + +#ifdef RTABMAP_REALSENSE_SLAM + motionSeq_[0] = motionSeq_[1] = 0; + if(slam_) + { + UScopeMutex lock(slamLock_); + UDEBUG("Flush slam"); + slam_->flush_resources(); + delete slam_; + slam_ = 0; + } +#endif + if (ctx_ == 0) { ctx_ = new rs::context(); @@ -2021,12 +2105,23 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str UERROR("Cannot connect to device %d", deviceId_); return false; } - UINFO("Using device %d, an %s", deviceId_, dev_->get_name()); + std::string name = dev_->get_name(); + UINFO("Using device %d, an %s", deviceId_, name.c_str()); UINFO(" Serial number: %s", dev_->get_serial()); UINFO(" Firmware version: %s", dev_->get_firmware_version()); UINFO(" Preset RGB: %d", presetRGB_); UINFO(" Preset Depth: %d", presetDepth_); +#ifdef RTABMAP_REALSENSE_SLAM + bool computeOdometry = false; + if (name.find("ZR300") != std::string::npos && computeOdometry_) + { + // Only enable ZR300 functionality if fisheye stream is enabled. + // Accel/Gyro automatically enabled when fisheye requested + computeOdometry = true; + } +#endif + // Configure depth and color to run with the device's preferred settings UINFO("Enabling streams..."); // R200: @@ -2040,12 +2135,258 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str rs::intrinsics color_intrin = dev_->get_stream_intrinsics(rs::stream::color); UINFO(" RGB: %dx%d", color_intrin.width, color_intrin.height); UINFO(" Depth: %dx%d", depth_intrin.width, depth_intrin.height); - dev_->start(); - dev_->wait_for_frames(); + UDEBUG("Setup frame callback"); + // Define lambda callback for receiving stream data + std::function frameCallback = [this](rs::frame frame) + { +#ifdef RTABMAP_REALSENSE_SLAM + if(slam_ != 0) + { + const auto timestampDomain = frame.get_frame_timestamp_domain(); + if (rs::timestamp_domain::microcontroller != timestampDomain) + { + UERROR("error: Junk time stamp in stream: %d\twith frame counter: %d", + (int)(frame.get_stream_type()), frame.get_frame_number()); + return ; + } + } +#endif + + int width = frame.get_width(); + int height = frame.get_height(); + rs::core::correlated_sample_set sample_set = {}; + + rs::core::image_info info = + { + width, + height, + rs::utils::convert_pixel_format(frame.get_format()), + frame.get_stride() + }; + cv::Mat image; + if(frame.get_format() == rs::format::raw8) + { + image = cv::Mat(height, width, CV_8UC1, (unsigned char*)frame.get_data()); + } + else if(frame.get_format() == rs::format::z16) + { + image = cv::Mat(height, width, CV_16UC1, (unsigned char*)frame.get_data()); + if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end()) + { + bufferedFrames_.find(frame.get_timestamp())->second.second = image.clone(); + UScopeMutex lock(dataMutex_); + bool notify = lastSyncFrames_.first.empty(); + lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second; + if(notify) + { + dataReady_.release(); + } + bufferedFrames_.erase(frame.get_timestamp()); + } + else + { + bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(cv::Mat(), image.clone()))); + } + if(bufferedFrames_.size()>5) + { + UWARN("Frames cannot be synchronized!"); + bufferedFrames_.clear(); + } + } + else if(frame.get_format() == rs::format::rgb8) + { + image = cv::Mat(height, width, CV_8UC3, (unsigned char*)frame.get_data()); + if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end()) + { + bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone(); + UScopeMutex lock(dataMutex_); + bool notify = lastSyncFrames_.first.empty(); + lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second; + if(notify) + { + dataReady_.release(); + } + bufferedFrames_.erase(frame.get_timestamp()); + } + else + { + bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(image.clone(), cv::Mat()))); + } + if(bufferedFrames_.size()>5) + { + UWARN("Frames cannot be synchronized!"); + bufferedFrames_.clear(); + } + return; + } + else + { + return; + } +#ifdef RTABMAP_REALSENSE_SLAM + if(slam_ != 0) + { + rs::core::stream_type stream = rs::utils::convert_stream_type(frame.get_stream_type()); + sample_set[stream] = rs::core::image_interface::create_instance_from_raw_data( + & info, + image.data, + stream, + rs::core::image_interface::flag::any, + frame.get_timestamp(), + (uint64_t)frame.get_frame_number(), + rs::core::timestamp_domain::microcontroller); + + UScopeMutex lock(slamLock_); + if (slam_->process_sample_set(sample_set) < rs::core::status_no_error) + { + UERROR("error: failed to process sample"); + } + sample_set[stream]->release(); + } +#endif + }; + + // Setup stream callback for stream + if(computeOdometry) + { + dev_->set_frame_callback(rs::stream::fisheye, frameCallback); + } + dev_->set_frame_callback(rs::stream::depth, frameCallback); + dev_->set_frame_callback(rs::stream::color, frameCallback); + + +#ifdef RTABMAP_REALSENSE_SLAM + if (computeOdometry) + { + dev_->enable_stream(rs::stream::fisheye, 640, 480, rs::format::raw8, 30); + rs::intrinsics fisheye_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye); + UINFO(" Fish: %dx%d", fisheye_intrin.width, fisheye_intrin.height); + + // Needed to align image timestamps to common clock-domain with the motion events + dev_->set_option(rs::option::fisheye_strobe, 1); + // This option causes the fisheye image to be aquired in-sync with the depth image. + dev_->set_option(rs::option::fisheye_external_trigger, 1); + dev_->set_option(rs::option::fisheye_color_auto_exposure, 1); + + UDEBUG("Setup motion callback"); + //define callback to the motion events and set it. + std::function motion_callback; + motion_callback = [this](rs::motion_data entry) + { + if ((entry.timestamp_data.source_id != RS_EVENT_IMU_GYRO) && + (entry.timestamp_data.source_id != RS_EVENT_IMU_ACCEL)) + return; + + rs_event_source motionType = entry.timestamp_data.source_id; + + rs::core::correlated_sample_set sample_set = {}; + if (motionType == RS_EVENT_IMU_ACCEL) + { + sample_set[rs::core::motion_type::accel].timestamp = entry.timestamp_data.timestamp; + sample_set[rs::core::motion_type::accel].data[0] = (float)entry.axes[0]; + sample_set[rs::core::motion_type::accel].data[1] = (float)entry.axes[1]; + sample_set[rs::core::motion_type::accel].data[2] = (float)entry.axes[2]; + sample_set[rs::core::motion_type::accel].type = rs::core::motion_type::accel; + ++motionSeq_[0]; + sample_set[rs::core::motion_type::accel].frame_number = motionSeq_[0]; + } + else if (motionType == RS_EVENT_IMU_GYRO) + { + sample_set[rs::core::motion_type::gyro].timestamp = entry.timestamp_data.timestamp; + sample_set[rs::core::motion_type::gyro].data[0] = (float)entry.axes[0]; + sample_set[rs::core::motion_type::gyro].data[1] = (float)entry.axes[1]; + sample_set[rs::core::motion_type::gyro].data[2] = (float)entry.axes[2]; + sample_set[rs::core::motion_type::gyro].type = rs::core::motion_type::gyro; + ++motionSeq_[1]; + sample_set[rs::core::motion_type::gyro].frame_number = motionSeq_[1]; + } + + UScopeMutex lock(slamLock_); + if (slam_->process_sample_set(sample_set) < rs::core::status_no_error) + { + UERROR("error: failed to process sample"); + } + }; + + std::function timestamp_callback; + timestamp_callback = [](rs::timestamp_data entry) {}; + + dev_->enable_motion_tracking(motion_callback, timestamp_callback); + UINFO(" enabled accel and gyro stream"); + + rs::motion_intrinsics imuIntrinsics; + rs::extrinsics fisheye2ImuExtrinsics; + rs::extrinsics fisheye2DepthExtrinsics; + try + { + imuIntrinsics = dev_->get_motion_intrinsics(); + fisheye2ImuExtrinsics = dev_->get_motion_extrinsics_from(rs::stream::fisheye); + fisheye2DepthExtrinsics = dev_->get_extrinsics(rs::stream::depth, rs::stream::fisheye); + } + catch (const rs::error & e) { + UERROR("Exception: %s (try to unplug/plug the camera)", e.what()); + return false; + } + + UDEBUG("Setup SLAM"); + UScopeMutex lock(slamLock_); + slam_ = new rs::slam::slam(); + slam_->set_auto_occupancy_map_building(false); + slam_->force_relocalization_pose(false); + + rs::core::video_module_interface::supported_module_config supported_config = {}; + if (slam_->query_supported_module_config(0, supported_config) < rs::core::status_no_error) + { + UERROR("Failed to query the first supported module configuration"); + return false; + } + + rs::core::video_module_interface::actual_module_config actual_config = {}; + + // Set camera intrinsics + std::map< rs::core::stream_type, rs::core::intrinsics > intrinsics; + intrinsics[rs::core::stream_type::fisheye] = rs::utils::convert_intrinsics(fisheye_intrin); + intrinsics[rs::core::stream_type::depth] = rs::utils::convert_intrinsics(depth_intrin); + + if(!setStreamConfigIntrin(rs::core::stream_type::fisheye, intrinsics, supported_config, actual_config)) + { + return false; + } + if(!setStreamConfigIntrin(rs::core::stream_type::depth, intrinsics, supported_config, actual_config)) + { + return false; + } + + // Set IMU intrinsics + actual_config[rs::core::motion_type::accel].is_enabled = true; + actual_config[rs::core::motion_type::gyro].is_enabled = true; + actual_config[rs::core::motion_type::gyro].intrinsics = rs::utils::convert_motion_device_intrinsics(imuIntrinsics.gyro); + actual_config[rs::core::motion_type::accel].intrinsics = rs::utils::convert_motion_device_intrinsics(imuIntrinsics.acc); + + // Set extrinsics + actual_config[rs::core::stream_type::fisheye].extrinsics_motion = rs::utils::convert_extrinsics(fisheye2ImuExtrinsics); + actual_config[rs::core::stream_type::fisheye].extrinsics = rs::utils::convert_extrinsics(fisheye2DepthExtrinsics); + + UDEBUG("Set SLAM config"); + // Set actual config + if (slam_->set_module_config(actual_config) < rs::core::status_no_error) + { + UERROR("error : failed to set the enabled module configuration"); + return false; + } + + dev_->start(rs::source::all_sources); + } + else +#endif + { + dev_->start(); + } + uSleep(1000); // ignore the first frames UINFO("Enabling streams...done!"); - + return true; #else @@ -2074,17 +2415,52 @@ std::string CameraRealSense::getSerial() const return "NA"; } +bool CameraRealSense::odomProvided() const +{ +#ifdef RTABMAP_REALSENSE_SLAM + return slam_!=0; +#else + return false; +#endif +} + +#ifdef RTABMAP_REALSENSE_SLAM +Transform rsPoseToTransform(const rs::slam::PoseMatrix4f & pose) +{ + return Transform( + pose.m_data[0], pose.m_data[1], pose.m_data[2], pose.m_data[3], + pose.m_data[4], pose.m_data[5], pose.m_data[6], pose.m_data[7], + pose.m_data[8], pose.m_data[9], pose.m_data[10], pose.m_data[11]); +} +#endif + SensorData CameraRealSense::captureImage(CameraInfo * info) { SensorData data; #ifdef RTABMAP_REALSENSE if (dev_) { - dev_->wait_for_frames(); + if(!dataReady_.acquire(1, 5000)) + { + UWARN("Not received new frames since 5 seconds, end of stream reached!"); + return data; + } - // Retrieve our images - const uint16_t * depth_image = (const uint16_t *)dev_->get_frame_data(rs::stream::depth); - const uint8_t * color_image = (const uint8_t *)dev_->get_frame_data(rs::stream::color); + cv::Mat rgb; + cv::Mat depthIn; + + { + UScopeMutex lock(dataMutex_); + rgb = lastSyncFrames_.first; + depthIn = lastSyncFrames_.second; + lastSyncFrames_.first = cv::Mat(); + lastSyncFrames_.second = cv::Mat(); + } + + if(rgb.empty() || depthIn.empty()) + { + return data; + } // Retrieve camera parameters for mapping between depth and color rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth); @@ -2093,7 +2469,6 @@ SensorData CameraRealSense::captureImage(CameraInfo * info) float scale = dev_->get_depth_scale(); // factory registration... - cv::Mat rgb = cv::Mat(cv::Size(color_intrin.width, color_intrin.height), CV_8UC3, (void*)color_image); cv::Mat bgr; cv::cvtColor(rgb, bgr, CV_RGB2BGR); @@ -2132,7 +2507,7 @@ SensorData CameraRealSense::captureImage(CameraInfo * info) for (int dx = 0; dx < depth_intrin.width; ++dx) { // Retrieve the 16-bit depth value and map it into a depth in meters - uint16_t depth_value = depth_image[dy * depth_intrin.width + dx]; + uint16_t depth_value = depthIn.at(dy,dx); float depth_in_meters = depth_value * scale; // Skip over pixels with a depth value of zero, which is used to indicate no data @@ -2165,6 +2540,22 @@ SensorData CameraRealSense::captureImage(CameraInfo * info) if (!bgr.empty() && !depth.empty()) { data = SensorData(bgr, depth, model, this->getNextSeqID(), UTimer::now()); +#ifdef RTABMAP_REALSENSE_SLAM + if(info && slam_) + { + UScopeMutex lock(slamLock_); + rs::slam::PoseMatrix4f pose; + if(slam_->get_camera_pose(pose) == rs::core::status_no_error) + { + Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0); + info->odomPose = opticalRotation * rsPoseToTransform(pose) * opticalRotation.inverse(); + } + else + { + UERROR("Failed getting odometry pose"); + } + } +#endif } } else diff --git a/examples/RGBDMapping/main.cpp b/examples/RGBDMapping/main.cpp index 38c95274..2bd99b7d 100644 --- a/examples/RGBDMapping/main.cpp +++ b/examples/RGBDMapping/main.cpp @@ -138,7 +138,7 @@ int main(int argc, char * argv[]) UERROR("Not built with RealSense support..."); exit(-1); } - camera = new CameraRealSense(0, 0, 0, 0, opticalRotation); + camera = new CameraRealSense(0, 0, 0, false, 0, opticalRotation); } else { diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 2d04c7e0..4193c1c1 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -154,6 +154,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->groupBox_octomap->setEnabled(false); #endif +#ifndef RTABMAP_REALSENSE_SLAM + _ui->checkbox_realsenseOdom->setChecked(false); + _ui->checkbox_realsenseOdom->setEnabled(false); +#endif + #ifndef RTABMAP_NONFREE _ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1); _ui->comboBox_detector_strategy->setItemData(1, 0, Qt::UserRole - 1); @@ -488,6 +493,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->checkBox_freenect2NoiseFiltering, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_realsensePresetRGB, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->comboBox_realsensePresetDepth, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkbox_realsenseOdom, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->toolButton_cameraImages_timestamps, SIGNAL(clicked()), this, SLOT(selectSourceImagesStamps())); connect(_ui->lineEdit_cameraImages_timestamps, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); @@ -1400,6 +1406,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkBox_freenect2NoiseFiltering->setChecked(true); _ui->comboBox_realsensePresetRGB->setCurrentIndex(0); _ui->comboBox_realsensePresetDepth->setCurrentIndex(2); + _ui->checkbox_realsenseOdom->setChecked(false); _ui->lineEdit_openniOniPath->clear(); _ui->lineEdit_openni2OniPath->clear(); _ui->lineEdit_cameraRGBDImages_path_rgb->setText(""); @@ -1767,6 +1774,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) settings.beginGroup("RealSense"); _ui->comboBox_realsensePresetRGB->setCurrentIndex(settings.value("presetRGB", _ui->comboBox_realsensePresetRGB->currentIndex()).toInt()); _ui->comboBox_realsensePresetDepth->setCurrentIndex(settings.value("presetDepth", _ui->comboBox_realsensePresetDepth->currentIndex()).toInt()); + _ui->checkbox_realsenseOdom->setChecked(settings.value("odom", _ui->checkbox_realsenseOdom->isChecked()).toBool()); settings.endGroup(); // RealSense settings.beginGroup("RGBDImages"); @@ -2152,6 +2160,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.beginGroup("RealSense"); settings.setValue("presetRGB", _ui->comboBox_realsensePresetRGB->currentIndex()); settings.setValue("presetDepth", _ui->comboBox_realsensePresetDepth->currentIndex()); + settings.setValue("odom", _ui->checkbox_realsenseOdom->isChecked()); settings.endGroup(); // RealSense settings.beginGroup("RGBDImages"); @@ -4518,6 +4527,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor) this->getSourceDevice().isEmpty() ? 0 : atoi(this->getSourceDevice().toStdString().c_str()), _ui->comboBox_realsensePresetRGB->currentIndex(), _ui->comboBox_realsensePresetDepth->currentIndex(), + _ui->checkbox_realsenseOdom->isChecked(), this->getGeneralInputRate(), this->getSourceLocalTransform()); } diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 4c82000f..6f58dbf8 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -641 + -416 678 2701 @@ -86,7 +86,7 @@ QFrame::Raised - 20 + 5 @@ -2582,7 +2582,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 1 + 0 @@ -2692,7 +2692,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - 1 + 6 @@ -3186,6 +3186,19 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki RealSense + + + + Qt::Vertical + + + + 20 + 0 + + + + @@ -3199,19 +3212,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - - - - Qt::Vertical - - - - 20 - 0 - - - - @@ -3269,6 +3269,29 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + Use visual inertial odometry for ZR300. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + false + + +