CameraRealSense.cpp

Go to the documentation of this file.

/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
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 Universite de Sherbrooke 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
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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 <rtabmap/core/camera/CameraRealSense.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UThreadC.h>
#include <rtabmap/core/util2d.h>
#include <opencv2/imgproc/types_c.h>
 
#ifdef RTABMAP_REALSENSE
#include <librealsense/rs.hpp>
#ifdef RTABMAP_REALSENSE_SLAM
#include <rs_core.h>
#include <rs_utils.h>
#include <librealsense/slam/slam.h>
#endif
#endif
 
 
namespace rtabmap
{
 
bool CameraRealSense::available()
{
#ifdef RTABMAP_REALSENSE
        return true;
#else
        return false;
#endif
}
 
CameraRealSense::CameraRealSense(
                int device,
                int presetRGB,
                int presetDepth,
                bool computeOdometry,
                float imageRate,
                const rtabmap::Transform & localTransform) :
        Camera(imageRate, localTransform)
#ifdef RTABMAP_REALSENSE
    ,
        ctx_(0),
        dev_(0),
        deviceId_(device),
        presetRGB_(presetRGB),
        presetDepth_(presetDepth),
        computeOdometry_(computeOdometry),
        depthScaledToRGBSize_(false),
        rgbSource_(kColor),
        slam_(0)
#endif
{
        UDEBUG("");
}
 
CameraRealSense::~CameraRealSense()
{
        UDEBUG("");
#ifdef RTABMAP_REALSENSE
        UDEBUG("");
        if(dev_)
        {
                try
                {
                        if(slam_!=0)
                        {
                                dev_->stop(rs::source::all_sources);
                        }
                        else
                        {
                                dev_->stop();
                        }
                }
                catch(const rs::error & error){UWARN("%s", error.what());}
                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
 
void CameraRealSense::setDepthScaledToRGBSize(bool enabled) {
#ifdef RTABMAP_REALSENSE
        depthScaledToRGBSize_ = enabled;
#endif
}
void CameraRealSense::setRGBSource(RGBSource source)
{
#ifdef RTABMAP_REALSENSE
        rgbSource_ = source;
#endif
}
 
#ifdef RTABMAP_REALSENSE
template<class GET_DEPTH, class TRANSFER_PIXEL> void align_images(const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, GET_DEPTH get_depth, TRANSFER_PIXEL transfer_pixel)
{
        // Iterate over the pixels of the depth image
#pragma omp parallel for schedule(dynamic)
        for(int depth_y = 0; depth_y < depth_intrin.height; ++depth_y)
        {
                int depth_pixel_index = depth_y * depth_intrin.width;
                for(int depth_x = 0; depth_x < depth_intrin.width; ++depth_x, ++depth_pixel_index)
                {
                        // Skip over depth pixels with the value of zero, we have no depth data so we will not write anything into our aligned images
                        if(float depth = get_depth(depth_pixel_index))
                        {
                                // Map the top-left corner of the depth pixel onto the other image
                                float depth_pixel[2] = {depth_x-0.5f, depth_y-0.5f}, depth_point[3], other_point[3], other_pixel[2];
                                rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
                                rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
                                rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
                                const int other_x0 = static_cast<int>(other_pixel[0] + 0.5f);
                                const int other_y0 = static_cast<int>(other_pixel[1] + 0.5f);
 
                                // Map the bottom-right corner of the depth pixel onto the other image
                                depth_pixel[0] = depth_x+0.5f; depth_pixel[1] = depth_y+0.5f;
                                rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
                                rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
                                rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
                                const int other_x1 = static_cast<int>(other_pixel[0] + 0.5f);
                                const int other_y1 = static_cast<int>(other_pixel[1] + 0.5f);
 
                                if(other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height) continue;
 
                                // Transfer between the depth pixels and the pixels inside the rectangle on the other image
                                for(int y=other_y0; y<=other_y1; ++y) for(int x=other_x0; x<=other_x1; ++x) transfer_pixel(depth_pixel_index, y * other_intrin.width + x);
                        }
                }
        }
}
typedef uint8_t byte;
 
void align_z_to_other(byte * z_aligned_to_other, const uint16_t * z_pixels, float z_scale, const rs_intrinsics & z_intrin, const rs_extrinsics & z_to_other, const rs_intrinsics & other_intrin)
{
        auto out_z = (uint16_t *)(z_aligned_to_other);
        align_images(z_intrin, z_to_other, other_intrin,
                [z_pixels, z_scale](int z_pixel_index) { return z_scale * z_pixels[z_pixel_index]; },
                [out_z, z_pixels](int z_pixel_index, int other_pixel_index) { out_z[other_pixel_index] = out_z[other_pixel_index] ? std::min(out_z[other_pixel_index],z_pixels[z_pixel_index]) : z_pixels[z_pixel_index]; });
}
 
void align_disparity_to_other(byte * disparity_aligned_to_other, const uint16_t * disparity_pixels, float disparity_scale, const rs_intrinsics & disparity_intrin, const rs_extrinsics & disparity_to_other, const rs_intrinsics & other_intrin)
{
        auto out_disparity = (uint16_t *)(disparity_aligned_to_other);
        align_images(disparity_intrin, disparity_to_other, other_intrin,
                [disparity_pixels, disparity_scale](int disparity_pixel_index) { return disparity_scale / disparity_pixels[disparity_pixel_index]; },
                [out_disparity, disparity_pixels](int disparity_pixel_index, int other_pixel_index) { out_disparity[other_pixel_index] = disparity_pixels[disparity_pixel_index]; });
}
 
template<int N> struct bytes { char b[N]; };
template<int N, class GET_DEPTH> void align_other_to_depth_bytes(byte * other_aligned_to_depth, GET_DEPTH get_depth, const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, const byte * other_pixels)
{
        auto in_other = (const bytes<N> *)(other_pixels);
        auto out_other = (bytes<N> *)(other_aligned_to_depth);
        align_images(depth_intrin, depth_to_other, other_intrin, get_depth,
                [out_other, in_other](int depth_pixel_index, int other_pixel_index) { out_other[depth_pixel_index] = in_other[other_pixel_index]; });
}
 
// Image rectification //
 
std::vector<int> compute_rectification_table(const rs_intrinsics & rect_intrin, const rs_extrinsics & rect_to_unrect, const rs_intrinsics & unrect_intrin)
{
        std::vector<int> rectification_table;
        rectification_table.resize(rect_intrin.width * rect_intrin.height);
        align_images(rect_intrin, rect_to_unrect, unrect_intrin, [](int) { return 1.0f; },
                [&rectification_table](int rect_pixel_index, int unrect_pixel_index) { rectification_table[rect_pixel_index] = unrect_pixel_index; });
        return rectification_table;
}
 
template<class T> void rectify_image_pixels(T * rect_pixels, const std::vector<int> & rectification_table, const T * unrect_pixels)
{
        for(auto entry : rectification_table) *rect_pixels++ = unrect_pixels[entry];
}
 
void rectify_image(uint8_t * rect_pixels, const std::vector<int> & rectification_table, const uint8_t * unrect_pixels, rs_format format)
{
        switch(format)
        {
        case RS_FORMAT_Y8:
                return rectify_image_pixels((bytes<1> *)rect_pixels, rectification_table, (const bytes<1> *)unrect_pixels);
        case RS_FORMAT_Y16: case RS_FORMAT_Z16:
                return rectify_image_pixels((bytes<2> *)rect_pixels, rectification_table, (const bytes<2> *)unrect_pixels);
        case RS_FORMAT_RGB8: case RS_FORMAT_BGR8:
                return rectify_image_pixels((bytes<3> *)rect_pixels, rectification_table, (const bytes<3> *)unrect_pixels);
        case RS_FORMAT_RGBA8: case RS_FORMAT_BGRA8:
                return rectify_image_pixels((bytes<4> *)rect_pixels, rectification_table, (const bytes<4> *)unrect_pixels);
        default:
                assert(false); // NOTE: rectify_image_pixels(...) is not appropriate for RS_FORMAT_YUYV images, no logic prevents U/V channels from being written to one another
        }
}
#endif
 
bool CameraRealSense::init(const std::string & calibrationFolder, const std::string & cameraName)
{
        UDEBUG("");
#ifdef RTABMAP_REALSENSE
 
        if(dev_)
        {
                try
                {
                        if(slam_!=0)
                        {
                                dev_->stop(rs::source::all_sources);
                        }
                        else
                        {
                                dev_->stop();
                        }
                }
                catch(const rs::error & error){UWARN("%s", error.what());}
                dev_ = 0;
        }
        bufferedFrames_.clear();
        rsRectificationTable_.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();
        }
 
        UDEBUG("");
        if (ctx_->get_device_count() == 0)
        {
                UERROR("No RealSense device detected!");
                return false;
        }
 
        UDEBUG("");
        dev_ = ctx_->get_device(deviceId_);
        if (dev_ == 0)
        {
                UERROR("Cannot connect to device %d", deviceId_);
                return false;
        }
        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_);
 
#ifndef RTABMAP_REALSENSE_SLAM
        computeOdometry_ = false;
#endif
 
        if (name.find("ZR300") == std::string::npos)
        {
                // Only enable ZR300 functionality odometry if fisheye stream is enabled.
                // Accel/Gyro automatically enabled when fisheye requested
                computeOdometry_ = false;
                // Only ZR300 has fisheye
                if(rgbSource_ == kFishEye)
                {
                        UWARN("Fisheye cannot be used with %s camera, using color instead...", name.c_str());
                        rgbSource_ = kColor;
                }
        }
 
        rs::intrinsics depth_intrin;
        rs::intrinsics fisheye_intrin;
        rs::intrinsics color_intrin;
        // Configure depth and color to run with the device's preferred settings
        UINFO("Enabling streams...");
        // R200: 
        //    0=640x480   vs 480x360
        //    1=1920x1080 vs 640x480
        //    2=640x480   vs 320x240
        try {
 
                // left/rgb stream
                if(rgbSource_==kFishEye || computeOdometry_)
                {
                        dev_->enable_stream(rs::stream::fisheye, 640, 480, rs::format::raw8, 30);
                        if(computeOdometry_)
                        {
                                // 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 acquired 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);
                        fisheye_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
                        UINFO("    Fisheye:   %dx%d", fisheye_intrin.width, fisheye_intrin.height);
                        if(rgbSource_==kFishEye)
                        {
                                color_intrin = fisheye_intrin; // not rectified
                        }
                }
                if(rgbSource_!=kFishEye)
                {
                        dev_->enable_stream(rs::stream::color, (rs::preset)presetRGB_);
                        color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color); // rectified
                        UINFO("    RGB:   %dx%d", color_intrin.width, color_intrin.height);
 
                        if(rgbSource_==kInfrared)
                        {
                                dev_->enable_stream(rs::stream::infrared, (rs::preset)presetRGB_);
                                color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared); // rectified
                                UINFO("    IR left:   %dx%d", color_intrin.width, color_intrin.height);
                        }
                }
 
                dev_->enable_stream(rs::stream::depth, (rs::preset)presetDepth_);
                depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth); // rectified
                UINFO("    Depth: %dx%d", depth_intrin.width, depth_intrin.height);
        }
        catch(const rs::error & error)
        {
                UERROR("Failed starting the streams: %s", error.what());
                return false;
        }
 
        Transform imu2Camera = Transform::getIdentity();
 
#ifdef RTABMAP_REALSENSE_SLAM
        UDEBUG("Setup frame callback");
        // Define lambda callback for receiving stream data
        std::function<void(rs::frame)> frameCallback = [this](rs::frame frame)
        {
                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 ;
                        }
                }
 
                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 || frame.get_format() == rs::format::y8)
                {
                        image = cv::Mat(height, width, CV_8UC1, (unsigned char*)frame.get_data());
                        if(frame.get_stream_type() == rs::stream::fisheye)
                        {
                                // fisheye always received just after the depth image (doesn't have exact timestamp with depth)
                                if(bufferedFrames_.size())
                                {
                                        bufferedFrames_.rbegin()->second.first = image.clone();
                                        UScopeMutex lock(dataMutex_);
                                        bool notify = lastSyncFrames_.first.empty();
                                        lastSyncFrames_ = bufferedFrames_.rbegin()->second;
                                        if(notify)
                                        {
                                                dataReady_.release();
                                        }
                                        bufferedFrames_.clear();
                                }
                        }
                        else if(frame.get_stream_type() == rs::stream::infrared) // infrared (does have exact timestamp with depth)
                        {
                                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 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)
                {
                        if(rsRectificationTable_.size())
                        {
                                image = cv::Mat(height, width, CV_8UC3);
                                rectify_image(image.data, rsRectificationTable_, (unsigned char*)frame.get_data(), (rs_format)frame.get_format());
                        }
                        else
                        {
                                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;
                }
 
                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();
                }
        };
 
        UDEBUG("");
        // Setup stream callback for stream
        if(computeOdometry_ || rgbSource_ == kFishEye)
        {
                dev_->set_frame_callback(rs::stream::fisheye, frameCallback);
        }
        if(rgbSource_ == kInfrared)
        {
                dev_->set_frame_callback(rs::stream::infrared, frameCallback);
        }
        else if(rgbSource_ == kColor)
        {
                dev_->set_frame_callback(rs::stream::color, frameCallback);
        }
 
        dev_->set_frame_callback(rs::stream::depth, frameCallback);
 
        UDEBUG("");
 
        if (computeOdometry_)
        {
                UDEBUG("Setup motion callback");
                //define callback to the motion events and set it.
                std::function<void(rs::motion_data)> 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<void(rs::timestamp_data)> 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;
                }
 
                rs::extrinsics fisheye2imu = dev_->get_motion_extrinsics_from(rs::stream::fisheye);
                imu2Camera = Transform(
                                fisheye2imu.rotation[0], fisheye2imu.rotation[1], fisheye2imu.rotation[2], fisheye2imu.translation[0],
                                fisheye2imu.rotation[3], fisheye2imu.rotation[4], fisheye2imu.rotation[5], fisheye2imu.translation[1],
                                fisheye2imu.rotation[6], fisheye2imu.rotation[7], fisheye2imu.rotation[8], fisheye2imu.translation[2]).inverse();
 
                if(rgbSource_ == kInfrared)
                {
                        rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::infrared);
                        Transform fisheye2Color = Transform(
                                        color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
                                        color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
                                        color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
                        imu2Camera *= fisheye2Color;
                }
                else if(rgbSource_ == kColor)
                {
                        rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::rectified_color);
                        Transform fisheye2Color = Transform(
                                        color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
                                        color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
                                        color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
                        imu2Camera *= fisheye2Color;
                }
 
                UDEBUG("start device!");
                try
                {
                        dev_->start(rs::source::all_sources);
                }
                catch(const rs::error & error)
                {
                        UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
                        return false;
                }
        }
        else
        {
                UDEBUG("start device!");
                try
                {
                        dev_->start();
                }
                catch(const rs::error & error)
                {
                        UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
                        return false;
                }
        }
#else
        try {
                dev_->start();
                dev_->wait_for_frames();
        }
        catch (const rs::error & e)
        {
                UERROR("Exception: %s (try to unplug/plug the camera)", e.what());
        }
#endif
 
        cv::Mat D;
        if(rgbSource_ == kFishEye)
        {
                // ftheta/equidistant model
                D = cv::Mat::zeros(1,6,CV_64FC1);
                D.at<double>(0,0) = color_intrin.coeffs[0];
                D.at<double>(0,1) = color_intrin.coeffs[1];
                D.at<double>(0,4) = color_intrin.coeffs[2];
                D.at<double>(0,5) = color_intrin.coeffs[3];
        }
        else
        {
                // Brown-Conrady / radtan
                D = cv::Mat::zeros(1,5,CV_64FC1);
                D.at<double>(0,0) = color_intrin.coeffs[0];
                D.at<double>(0,1) = color_intrin.coeffs[1];
                D.at<double>(0,2) = color_intrin.coeffs[2];
                D.at<double>(0,3) = color_intrin.coeffs[3];
                D.at<double>(0,4) = color_intrin.coeffs[4];
        }
        cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
        K.at<double>(0,0) = color_intrin.fx;
        K.at<double>(1,1) = color_intrin.fy;
        K.at<double>(0,2) = color_intrin.ppx;
        K.at<double>(1,2) = color_intrin.ppy;
        cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
        cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
        K(cv::Range(0,2), cv::Range(0,3)).copyTo(P(cv::Range(0,2), cv::Range(0,3)));
        cameraModel_ = CameraModel(
                        dev_->get_name(),
                        cv::Size(color_intrin.width, color_intrin.height),
                        K,
                        D,
                        R,
                        P,
                        this->getLocalTransform()*imu2Camera);
 
        UDEBUG("");
 
        if(rgbSource_ == kColor)
        {
                rs::extrinsics rect_to_unrect = dev_->get_extrinsics(rs::stream::rectified_color, rs::stream::color);
                rs::intrinsics unrect_intrin = dev_->get_stream_intrinsics(rs::stream::color);
                rsRectificationTable_ = compute_rectification_table(color_intrin, rect_to_unrect, unrect_intrin);
        }
        else if(rgbSource_ == kFishEye)
        {
                UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
                if(!calibrationFolder.empty() && !cameraName.empty())
                {
                        CameraModel model;
                        if(!model.load(calibrationFolder, cameraName))
                        {
                                UWARN("Failed to load calibration \"%s\" in \"%s\" folder, you should calibrate the camera!",
                                                cameraName.c_str(), calibrationFolder.c_str());
                        }
                        else
                        {
                                UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
                                                model.fx(),
                                                model.fy(),
                                                model.cx(),
                                                model.cy());
                                cameraModel_ = model;
                                cameraModel_.setName(cameraName);
                                cameraModel_.initRectificationMap();
                                cameraModel_.setLocalTransform(this->getLocalTransform()*imu2Camera);
                        }
                }
        }
 
        uSleep(1000); // ignore the first frames
        UINFO("Enabling streams...done!");
 
        return true;
 
#else
        UERROR("CameraRealSense: RTAB-Map is not built with RealSense support!");
        return false;
#endif
}
 
bool CameraRealSense::isCalibrated() const
{
        return true;
}
 
std::string CameraRealSense::getSerial() const
{
#ifdef RTABMAP_REALSENSE
        if (dev_)
        {
                return dev_->get_serial();
        }
        else
        {
                UERROR("Cannot get a serial before initialization. Call init() before.");
        }
#endif
        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(SensorCaptureInfo * info)
{
        SensorData data;
#ifdef RTABMAP_REALSENSE
        if (dev_)
        {
                cv::Mat rgb;
                cv::Mat depthIn;
 
                // Retrieve camera parameters for mapping between depth and color
                rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
                rs::extrinsics depth_to_color;
                rs::intrinsics color_intrin;
                if(rgbSource_ == kFishEye)
                {
                        depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::fisheye);
                        color_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
                }
                else if(rgbSource_ == kInfrared)
                {
                        depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::infrared);
                        color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared);
                }
                else // color
                {
                        depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::rectified_color);
                        color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color);
                }
 
#ifdef RTABMAP_REALSENSE_SLAM
                if(!dataReady_.acquire(1, 5000))
                {
                        UWARN("Not received new frames since 5 seconds, end of stream reached!");
                        return data;
                }
 
                {
                        UScopeMutex lock(dataMutex_);
                        rgb = lastSyncFrames_.first;
                        depthIn = lastSyncFrames_.second;
                        lastSyncFrames_.first = cv::Mat();
                        lastSyncFrames_.second = cv::Mat();
                }
 
                if(rgb.empty() || depthIn.empty())
                {
                        return data;
                }
#else
                try {
                        dev_->wait_for_frames();
                }
                catch (const rs::error & e)
                {
                        UERROR("Exception: %s", e.what());
                        return data;
                }
 
                // Retrieve our images
                depthIn = cv::Mat(depth_intrin.height, depth_intrin.width, CV_16UC1, (unsigned char*)dev_->get_frame_data(rs::stream::depth));
                if(rgbSource_ == kFishEye)
                {
                        rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::fisheye));
                }
                else if(rgbSource_ == kInfrared)
                {
                        rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::infrared));
                }
                else
                {
                        rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC3, (unsigned char*)dev_->get_frame_data(rs::stream::color));
                }
#endif
 
                // factory registration...
                cv::Mat bgr;
                if(rgbSource_ != kColor)
                {
                        bgr = rgb;
                }
                else
                {
                        cv::cvtColor(rgb, bgr, CV_RGB2BGR);
                }
 
                bool rectified = false;
                if(rgbSource_ == kFishEye && cameraModel_.isRectificationMapInitialized())
                {
                        bgr = cameraModel_.rectifyImage(bgr);
                        rectified = true;
                        color_intrin.fx = cameraModel_.fx();
                        color_intrin.fy = cameraModel_.fy();
                        color_intrin.ppx = cameraModel_.cx();
                        color_intrin.ppy = cameraModel_.cy();
                        UASSERT_MSG(color_intrin.width == cameraModel_.imageWidth() && color_intrin.height == cameraModel_.imageHeight(),
                                        uFormat("color_intrin=%dx%d cameraModel_=%dx%d",
                                                        color_intrin.width, color_intrin.height, cameraModel_.imageWidth(), cameraModel_.imageHeight()).c_str());
                        ((rs_intrinsics*)&color_intrin)->model = RS_DISTORTION_NONE;
                }
#ifndef RTABMAP_REALSENSE_SLAM
                else if(rgbSource_ != kColor)
                {
                        bgr = bgr.clone();
                }
#endif
 
                cv::Mat depth;
                if(rgbSource_ != kFishEye || rectified)
                {
                        if (color_intrin.width % depth_intrin.width == 0 && color_intrin.height % depth_intrin.height == 0 &&
                                depth_intrin.width < color_intrin.width &&
                                depth_intrin.height < color_intrin.height &&
                                !depthScaledToRGBSize_)
                        {
                                //we can keep the depth image size as is
                                depth = cv::Mat::zeros(cv::Size(depth_intrin.width, depth_intrin.height), CV_16UC1);
                                float scaleX = float(depth_intrin.width) / float(color_intrin.width);
                                float scaleY = float(depth_intrin.height) / float(color_intrin.height);
                                color_intrin.fx *= scaleX;
                                color_intrin.fy *= scaleY;
                                color_intrin.ppx *= scaleX;
                                color_intrin.ppy *= scaleY;
                                color_intrin.height = depth_intrin.height;
                                color_intrin.width = depth_intrin.width;
                        }
                        else
                        {
                                //depth to color
                                depth = cv::Mat::zeros(bgr.size(), CV_16UC1);
                        }
 
                        float scale = dev_->get_depth_scale();
                        for (int dy = 0; dy < depth_intrin.height; ++dy)
                        {
                                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 = depthIn.at<unsigned short>(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
                                        if (depth_value == 0 || depth_in_meters>10.0f) continue;
 
                                        // Map from pixel coordinates in the depth image to pixel coordinates in the color image
                                        int pdx = dx;
                                        int pdy = dy;
                                        if(rgbSource_ == kColor || rgbSource_ == kFishEye)
                                        {
                                                rs::float2 depth_pixel = { (float)dx, (float)dy };
                                                rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
                                                rs::float3 color_point = depth_to_color.transform(depth_point);
                                                rs::float2 color_pixel = color_intrin.project(color_point);
 
                                                pdx = color_pixel.x;
                                                pdy = color_pixel.y;
                                        }
                                        //else infrared is already registered
 
                                        if (uIsInBounds(pdx, 0, depth.cols) && uIsInBounds(pdy, 0, depth.rows))
                                        {
                                                depth.at<unsigned short>(pdy, pdx) = (unsigned short)(depth_in_meters*1000.0f); // convert to mm
                                        }
                                }
                        }
 
                        if (color_intrin.width > depth_intrin.width)
                        {
                                // Fill holes
                                UTimer time;
                                util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);
                                util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);//second pass
                                UDEBUG("Filling depth holes: %fs", time.ticks());
                        }
                }
 
                if (!bgr.empty() && ((rgbSource_==kFishEye && !rectified) || !depth.empty()))
                {
                        data = SensorData(bgr, depth, cameraModel_, 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)
                                {
                                        /*rs::slam::tracking_accuracy accuracy = slam_->get_tracking_accuracy();
                                        if( accuracy == rs::slam::tracking_accuracy::low ||
                                                accuracy == rs::slam::tracking_accuracy::medium ||
                                                accuracy == rs::slam::tracking_accuracy::high)*/
                                        {
                                                // the pose is in camera link or IMU frame, get pose of the color camera
                                                Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
                                                info->odomPose = opticalRotation * rsPoseToTransform(pose) * opticalRotation.inverse();
                                                info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005;
                                        }
                                        /*else
                                        {
                                                UERROR("Odometry failed: accuracy=%d", accuracy);
                                        }*/
                                }
                                else
                                {
                                        UERROR("Failed getting odometry pose");
                                }
                        }
#endif
                }
        }
        else
        {
                ULOGGER_WARN("The camera must be initialized before requesting an image.");
        }
#else
        UERROR("CameraRealSense: RTAB-Map is not built with RealSense support!");
#endif
        return data;
}
 
} // namespace rtabmap