D435iCamera.cpp

#include "stdafx.h"
#include "Version.h"
#include "D435iCamera.h"
#include "Visualizer.h"

#include <librealsense2/rs.h>
#include <librealsense2/rs.hpp>
#include <librealsense2/rsutil.h>
#include <librealsense2/hpp/rs_pipeline.hpp>

/** RealSense SDK2 Cross-Platform Depth Camera Backend **/
namespace ark {

    D435iCamera::D435iCamera(): D435iCamera(CameraParameter()){}

    D435iCamera::D435iCamera(const CameraParameter &parameter): 
        cameraParameter(parameter), last_ts_g(0), kill(false) {
        //Setup camera
        //TODO: Make read from config file


		//rs400::advanced_mode advanced_device(camera.getDevice());
		//auto depth_table = advanced_device.get_depth_table();
		//depth_table.depthClampMax = 1300; // 1m30 if depth unit at 0.001
		//advanced_device.set_depth_table(depth_table);


        rs2::context ctx;
        device = ctx.query_devices().front();
        width = cameraParameter.width;
        height = cameraParameter.height;
        //Setup configuration
        const auto irDepthFps = cameraParameter.irDepthFps;
        config.enable_stream(RS2_STREAM_DEPTH,-1,width, height,RS2_FORMAT_Z16,irDepthFps);
        config.enable_stream(RS2_STREAM_INFRARED, 1, width, height, RS2_FORMAT_Y8, irDepthFps);
        config.enable_stream(RS2_STREAM_INFRARED, 2, width, height, RS2_FORMAT_Y8, irDepthFps);
        config.enable_stream(RS2_STREAM_COLOR, -1, width, height, RS2_FORMAT_RGB8, irDepthFps);
        const auto imuFps = cameraParameter.imuFps;
        // TODO: check if fps of acc is neccesary to be different from gyro
        motion_config.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F,imuFps+50);
        motion_config.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F,imuFps);
        //Need to get the depth sensor specifically as it is the one that controls the sync funciton
        depth_sensor = new rs2::depth_sensor(device.first<rs2::depth_sensor>());

        scale = depth_sensor->get_option(RS2_OPTION_DEPTH_UNITS);




		//depth_sensor->set_option(RS2_OPTION_MAX_DISTANCE, 16.0f);
			
        rs2::sensor color_sensor = device.query_sensors()[1];

        color_sensor.set_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE,false);

    }

    D435iCamera::~D435iCamera() {
        try {
            kill=true;
            imuReaderThread_.join();
            pipe->stop();
            if(depth_sensor){
                delete depth_sensor;
                depth_sensor=nullptr;
            }
        }
        catch (...) {}
    }

    void D435iCamera::start(){
        //enable sync
        //depth_sensor->set_option(RS2_OPTION_INTER_CAM_SYNC_MODE,1);
        depth_sensor->set_option(RS2_OPTION_EMITTER_ENABLED, cameraParameter.emitterPower);
        //start streaming
        pipe = std::make_shared<rs2::pipeline>();
        rs2::pipeline_profile selection = pipe->start(config);
        //get the depth intrinsics (needed for projection to 3d)
        auto depthStream = selection.get_stream(RS2_STREAM_DEPTH)
                             .as<rs2::video_stream_profile>();
        depthIntrinsics = depthStream.get_intrinsics();
		colorIntrinsics = selection.get_stream(RS2_STREAM_COLOR)
			.as<rs2::video_stream_profile>().get_intrinsics();

        motion_pipe = std::make_shared<rs2::pipeline>();
        rs2::pipeline_profile selection_motion = motion_pipe->start(motion_config);

        if (RS2_API_MAJOR_VERSION > 2 || RS2_API_MAJOR_VERSION == 2 && RS2_API_MINOR_VERSION >= 22) {
            
            auto dev = selection.get_device();
            auto sensors = dev.query_sensors();

            auto dev_motion = selection_motion.get_device();
            auto sensors_motion = dev_motion.query_sensors();

            int global_time_option = -1;
            string match = "Global Time Enabled";

            for (int i = 0; i < rs2_option::RS2_OPTION_COUNT; i++) {
                if (!strcmp(rs2_option_to_string((rs2_option)i), match.c_str())) {
                    global_time_option = i;
                    break;
                }
            }

            if (global_time_option == -1) {
                cout << "Couldn't find Global Time Enabled Option" << endl;
            }

            for (auto sensor: sensors) {
                sensor.set_option((rs2_option)global_time_option, false);
            }   

            for (auto sensor: sensors_motion) {
                sensor.set_option((rs2_option)global_time_option, false);
            }
        } 
        align_to_color = new rs2::align(RS2_STREAM_COLOR);
        imuReaderThread_ = std::thread(&D435iCamera::imuReader, this);
    }

	std::vector<float> D435iCamera::getColorIntrinsics() {
		return std::vector<float>{colorIntrinsics.fx, colorIntrinsics.fy, colorIntrinsics.ppx, colorIntrinsics.ppy};
	}

    void D435iCamera::imuReader(){
        while(!kill){
            auto frames = motion_pipe->wait_for_frames();
            auto fa = frames.first(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F)
                .as<rs2::motion_frame>();
            auto fg = frames.first(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F)
                .as<rs2::motion_frame>();

            double ts_g = fg.get_timestamp();
            if(ts_g != last_ts_g){
                last_ts_g=ts_g;
                // std::cout << "GYRO: " << ts_g / 1e2 << std::endl;
                // Get gyro measures
                rs2_vector gyro_data = fg.get_motion_data();

                // Get the timestamp of the current frame
                double ts_a = fa.get_timestamp();
                //std::cout << "ACCEL: " << ts_a  <<  std::endl;
                // Get accelerometer measures
                rs2_vector accel_data = fa.get_motion_data();

                ImuPair imu_out { double(ts_g)*1e6, //convert to nanoseconds, for some reason gyro timestamp is in centiseconds
                    Eigen::Vector3d(gyro_data.x,gyro_data.y,gyro_data.z),
                    Eigen::Vector3d(accel_data.x,accel_data.y,accel_data.z)};
                imu_queue_.enqueue(imu_out);
            }

        }

    }

    bool D435iCamera::getImuToTime(double timestamp, std::vector<ImuPair>& data_out){
        ImuPair imu_data;
        imu_data.timestamp=0;
        float imu_rate = static_cast<float>(cameraParameter.imuFps);
        while((imu_data.timestamp+1e9/imu_rate)<timestamp){
            if(imu_queue_.try_dequeue(&imu_data)){
                data_out.push_back(imu_data);
            }
        }
        return true;

    };



    const std::string D435iCamera::getModelName() const {
        return "RealSense";
    }

    cv::Size D435iCamera::getImageSize() const
    {
        return cv::Size(width,height);
    }

    void D435iCamera::update(MultiCameraFrame & frame) {

        try {
            // Ensure the frame has space for all images
            frame.images_.resize(5);

            // Get frames from camera
            auto frames = pipe->wait_for_frames();
            auto infrared = frames.get_infrared_frame(1);        
            auto infrared2 = frames.get_infrared_frame(2);
            auto depth = frames.get_depth_frame();
            auto color = frames.get_color_frame();

            // Store ID for later
            frame.frameId_ = depth.get_frame_number();
            if(depth.supports_frame_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP)){
                frame.timestamp_= depth.get_frame_metadata(RS2_FRAME_METADATA_SENSOR_TIMESTAMP)*1e3;
                //std::cout << "Image: " << std::fixed << frame.timestamp_/1e3 << std::endl;
            
            }else{
                std::cout << "No Metadata" << std::endl;
            }

            // Convert infrared frame to opencv
            if (frame.images_[0].empty()) frame.images_[0] = cv::Mat(cv::Size(width,height), CV_8UC1);
            std::memcpy( frame.images_[0].data, infrared.get_data(),width * height);

            if (frame.images_[1].empty()) frame.images_[1] = cv::Mat(cv::Size(width,height), CV_8UC1);
            std::memcpy( frame.images_[1].data, infrared2.get_data(),width * height);


            if (frame.images_[2].empty()) frame.images_[2] = cv::Mat(cv::Size(width,height), CV_32FC3);
            project(depth, frame.images_[2]);
            frame.images_[2] = frame.images_[2]*scale; //depth is in mm by default

			auto aligned_frames = align_to_color->process(frames);
			auto aligned_depth = aligned_frames.get_depth_frame();
			
			if (frame.images_[4].empty()) frame.images_[4] = cv::Mat(cv::Size(width, height), CV_16UC1, (void*)aligned_depth.get_data(), cv::Mat::AUTO_STEP);

            if (frame.images_[3].empty()) frame.images_[3] = cv::Mat(cv::Size(width,height), CV_8UC3);
            std::memcpy( frame.images_[3].data, color.get_data(),3 * width * height);

			//FILTER OUT ALL POINTS CLOSER THAN min_dist AND FARTHER THAN max_dist
			int min_dist = 200;
			int max_dist = 6000;
			for (int i = 0; i < width; i++) {
				for (int k = 0; k < height; k++) {
					if (frame.images_[4].at<uint16_t>(k, i) < min_dist || frame.images_[4].at<uint16_t>(k, i) > max_dist) { 
						frame.images_[4].at<uint16_t>(k, i) = 0;
					}
				}
			}

        } catch (std::runtime_error e) {
            // Try reconnecting
            badInputFlag = true;
            pipe->stop();
            printf("Couldn't connect to camera, retrying in 0.5s...\n");
            boost::this_thread::sleep_for(boost::chrono::milliseconds(500));
            //query_intrinsics();
            pipe->start(config);
            badInputFlag = false;
            return;
        }
    }

    // project depth map to xyz coordinates directly (faster and minimizes distortion, but will not be aligned to RGB/IR)
    void D435iCamera::project(const rs2::frame & depth_frame, cv::Mat & xyz_map) {
        const uint16_t * depth_data = (const uint16_t *)depth_frame.get_data();

        rs2_intrinsics * dIntrin = &depthIntrinsics;

        const uint16_t * srcPtr;
        cv::Vec3f * destPtr;
        float srcPixel[2], destXYZ[3];

        for (int r = 0; r < height; ++r)
        {
            srcPtr = depth_data + r * dIntrin->width;
            destPtr = xyz_map.ptr<Vec3f>(r);
            srcPixel[1] = r;

            for (int c = 0; c < width; ++c)
            {
                if (srcPtr[c] == 0) {
                    memset(&destPtr[c], 0, 3 * sizeof(float));
                    continue;
                }
                srcPixel[0] = c;
                rs2_deproject_pixel_to_point(destXYZ, dIntrin, srcPixel, srcPtr[c]);
                memcpy(&destPtr[c], destXYZ, 3 * sizeof(float));
            }
        }
    }

    const rs2_intrinsics &D435iCamera::getDepthIntrinsics() {
        return depthIntrinsics;
    }

    double D435iCamera::getDepthScale() {
        return scale;
    }
}