CDUO3DCamera.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include "hwdrivers-precomp.h"  // Precompiled headers

#include <mrpt/system/os.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/hwdrivers/CDUO3DCamera.h>

#include <mrpt/otherlibs/do_opencv_includes.h>

#include <map>

// We must define & store OpenCV-specific data like this in the .cpp, we don't
// want to force users to need opencv headers:
struct TDUOParams
{
#if MRPT_HAS_OPENCV
    cv::Mat m_rectify_map_left_x;
    cv::Mat m_rectify_map_left_y;
    cv::Mat m_rectify_map_right_x;
    cv::Mat m_rectify_map_right_y;
#endif
};
std::map<const mrpt::hwdrivers::TCaptureOptions_DUO3D*, TDUOParams> duo_params;

// duo3d header files
#if MRPT_HAS_DUO3D
#include <DUOLib.h>
#endif

// m_duo: Opaque pointer to DUO3D's "DUOInstance":
#define M_DUO_PTR (reinterpret_cast<DUOInstance*>(m_duo))
#define M_DUO_VALUE (*M_DUO_PTR)

using namespace std;
using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace mrpt::img;
using namespace mrpt::obs;
using namespace mrpt::hwdrivers;

// opencv header files and namespaces
#if MRPT_HAS_OPENCV
using namespace cv;
#endif

TCaptureOptions_DUO3D::TCaptureOptions_DUO3D()
    : m_img_width(640),
      m_img_height(480),
      m_fps(30),
      m_exposure(50),
      m_led(25),
      m_gain(0),
      m_capture_imu(false),
      m_capture_rectified(false),
      m_calibration_from_file(true),
      m_rectify_map_filename(""),
      m_intrinsic_filename(""),
      m_extrinsic_filename(""),
      m_stereo_camera(TStereoCamera())
{
    duo_params[this];  // Create
}

TCaptureOptions_DUO3D::~TCaptureOptions_DUO3D()
{
    duo_params.erase(this);  // Remove entry
}

TCaptureOptions_DUO3D::TYMLReadResult
    TCaptureOptions_DUO3D::m_rectify_map_from_yml(const string& _file_name)
{
#if MRPT_HAS_OPENCV
    const string file_name =
        _file_name.empty() ? m_rectify_map_filename : _file_name;

    TDUOParams& dp = duo_params[this];

    string aux = mrpt::system::extractFileName(file_name);
    const size_t found = aux.find(
        mrpt::format("_R%dx%d_", this->m_img_width, this->m_img_height));
    if (found == std::string::npos)
    {
        dp.m_rectify_map_left_x = dp.m_rectify_map_left_y =
            dp.m_rectify_map_right_x = dp.m_rectify_map_right_y = cv::Mat();
        return yrr_NAME_NON_CONSISTENT;
    }
    // read file
    FileStorage fs(file_name, FileStorage::READ);
    fs["R0X"] >> dp.m_rectify_map_left_x;
    fs["R0Y"] >> dp.m_rectify_map_left_y;
    fs["R1X"] >> dp.m_rectify_map_right_x;
    fs["R1Y"] >> dp.m_rectify_map_right_y;

    if (dp.m_rectify_map_left_x.size() == Size(0, 0) ||
        dp.m_rectify_map_left_y.size() == Size(0, 0) ||
        dp.m_rectify_map_right_x.size() == Size(0, 0) ||
        dp.m_rectify_map_right_y.size() == Size(0, 0))
        return yrr_EMPTY;

    return yrr_OK;
#else
    THROW_EXCEPTION("This function requires building with OpenCV support");
#endif
}

TCaptureOptions_DUO3D::TYMLReadResult
    TCaptureOptions_DUO3D::m_camera_ext_params_from_yml(
        const string& _file_name)
{
#if MRPT_HAS_OPENCV
    const string file_name =
        _file_name.empty() ? m_extrinsic_filename : _file_name;

    // this will look for R and t matrixes
    cv::Mat aux_mat;
    bool empty = false;
    string aux = mrpt::system::extractFileName(file_name);
    const size_t found = aux.find(
        mrpt::format("_R%dx%d_", this->m_img_width, this->m_img_height));
    if (found == std::string::npos)
    {
        m_stereo_camera.rightCameraPose = TPose3DQuat(0, 0, 0, 1.0, 0, 0, 0);
        return yrr_NAME_NON_CONSISTENT;
    }
    // read file
    FileStorage fs(file_name, FileStorage::READ);
    CMatrixDouble33 M;
    CMatrixDouble13 t;
    CMatrixDouble44 M2;

    // rotation matrix
    fs["R"] >> aux_mat;
    if (aux_mat.size() == Size(3, 3))
    {
        for (size_t k1 = 0; k1 < 3; ++k1)
            for (size_t k2 = 0; k2 < 3; ++k2)
                M(k1, k2) = aux_mat.at<double>(k1, k2);
    }
    else
    {
        empty = true;
        m_stereo_camera.rightCameraPose = TPose3DQuat(0, 0, 0, 1, 0, 0, 0);
    }

    // translation
    fs["T"] >> aux_mat;
    if (aux_mat.size() == Size(1, 3))
    {
        t(0, 0) = aux_mat.at<double>(0, 0) / 1000.0;
        t(0, 1) = aux_mat.at<double>(1, 0) / 1000.0;
        t(0, 2) = aux_mat.at<double>(2, 0) / 1000.0;
    }
    else
    {
        empty = true;
        m_stereo_camera.rightCameraPose = TPose3DQuat(0, 0, 0, 1, 0, 0, 0);
    }

    if (empty) return yrr_EMPTY;

    m_stereo_camera.rightCameraPose = CPose3DQuat(CPose3D(M, t)).asTPose();
    return yrr_OK;
#else
    THROW_EXCEPTION("This function requires building with OpenCV support");
#endif
}

TCaptureOptions_DUO3D::TYMLReadResult
    TCaptureOptions_DUO3D::m_camera_int_params_from_yml(
        const string& _file_name)
{
#if MRPT_HAS_OPENCV
    const string file_name =
        _file_name.empty() ? m_intrinsic_filename : _file_name;

    // this will look for M1, D1, M2 and D2 matrixes
    cv::Mat aux_mat;
    bool empty = false;
    string aux = mrpt::system::extractFileName(file_name);
    const size_t found = aux.find(
        mrpt::format("_R%dx%d_", this->m_img_width, this->m_img_height));
    if (found == std::string::npos)
    {
        m_stereo_camera.leftCamera.intrinsicParams.zeros();
        m_stereo_camera.leftCamera.dist.fill(0);
        m_stereo_camera.rightCamera.intrinsicParams.zeros();
        m_stereo_camera.rightCamera.dist.fill(0);

        return yrr_NAME_NON_CONSISTENT;
    }
    // read file
    FileStorage fs(file_name, FileStorage::READ);

    // left camera
    fs["M1"] >> aux_mat;
    if (aux_mat.size() == Size(0, 0))
    {
        empty = true;
        m_stereo_camera.leftCamera.intrinsicParams.zeros();
    }
    m_stereo_camera.leftCamera.setIntrinsicParamsFromValues(
        aux_mat.at<double>(0, 0), aux_mat.at<double>(1, 1),
        aux_mat.at<double>(0, 2), aux_mat.at<double>(1, 2));

    fs["D1"] >> aux_mat;
    if (aux_mat.size() == Size(0, 0))
    {
        empty = true;
        m_stereo_camera.leftCamera.dist.fill(0);
    }
    m_stereo_camera.leftCamera.setDistortionParamsFromValues(
        aux_mat.at<double>(0, 0), aux_mat.at<double>(0, 1),
        aux_mat.at<double>(0, 2), aux_mat.at<double>(0, 3),
        aux_mat.at<double>(0, 4));

    fs["M2"] >> aux_mat;
    if (aux_mat.size() == Size(0, 0))
    {
        empty = true;
        m_stereo_camera.rightCamera.intrinsicParams.zeros();
    }
    m_stereo_camera.rightCamera.setIntrinsicParamsFromValues(
        aux_mat.at<double>(0, 0), aux_mat.at<double>(1, 1),
        aux_mat.at<double>(0, 2), aux_mat.at<double>(1, 2));

    fs["D2"] >> aux_mat;
    if (aux_mat.size() == Size(0, 0))
    {
        empty = true;
        m_stereo_camera.rightCamera.dist.fill(0);
    }
    m_stereo_camera.rightCamera.setDistortionParamsFromValues(
        aux_mat.at<double>(0, 0), aux_mat.at<double>(0, 1),
        aux_mat.at<double>(0, 2), aux_mat.at<double>(0, 3),
        aux_mat.at<double>(0, 4));

    return empty ? yrr_EMPTY : yrr_OK;
#else
    THROW_EXCEPTION("This function requires building with OpenCV support");
#endif
}

void TCaptureOptions_DUO3D::loadOptionsFrom(
    const mrpt::config::CConfigFileBase& configSource,
    const std::string& iniSection, const std::string& prefix)
{
    m_img_width = configSource.read_int(iniSection, "image_width", m_img_width);
    m_img_height =
        configSource.read_int(iniSection, "image_height", m_img_height);

    m_fps = configSource.read_float(iniSection, "fps", m_fps);
    m_exposure = configSource.read_float(iniSection, "exposure", m_exposure);
    m_led = configSource.read_float(iniSection, "led", m_led);
    m_gain = configSource.read_float(iniSection, "gain", m_gain);

    m_capture_rectified = configSource.read_bool(
        iniSection, "capture_rectified", m_capture_rectified);
    m_capture_imu =
        configSource.read_bool(iniSection, "capture_imu", m_capture_imu);
    m_calibration_from_file = configSource.read_bool(
        iniSection, "calibration_from_file", m_calibration_from_file);

    if (m_calibration_from_file)
    {
        m_intrinsic_filename = configSource.read_string(
            iniSection, "intrinsic_filename", m_intrinsic_filename);
        m_extrinsic_filename = configSource.read_string(
            iniSection, "extrinsic_filename", m_extrinsic_filename);
        m_stereo_camera.leftCamera.ncols = m_stereo_camera.rightCamera.ncols =
            m_img_width;
        m_stereo_camera.leftCamera.nrows = m_stereo_camera.rightCamera.nrows =
            m_img_height;
    }
    else
        m_stereo_camera.loadFromConfigFile("DUO3D", configSource);

    if (m_capture_rectified)
    {
        m_rectify_map_filename = configSource.read_string(
            iniSection, "rectify_map_filename", m_rectify_map_filename);
    }  // end-capture-rectified
}

#if MRPT_HAS_DUO3D
static void CALLBACK DUOCallback(const PDUOFrame pFrameData, void* pUserData)
{
    CDUO3DCamera* obj = static_cast<CDUO3DCamera*>(pUserData);
    obj->setDataFrame(reinterpret_cast<void*>(pFrameData));
    SetEvent(reinterpret_cast<HANDLE>(obj->getEvent()));
}
#endif

/** Default constructor. */
CDUO3DCamera::CDUO3DCamera()
    : m_options(TCaptureOptions_DUO3D()), m_duo(nullptr)
{
#if MRPT_HAS_DUO3D
    m_duo = new DUOInstance[1];
    M_DUO_VALUE = nullptr;  // m_duo = nullptr;

    m_pframe_data = nullptr;
    m_evFrame = CreateEvent(nullptr, FALSE, FALSE, nullptr);
#else
    THROW_EXCEPTION(
        "MRPT has been compiled with 'MRPT_BUILD_DUO3D'=OFF, so this class "
        "cannot be used.");
#endif
}  // end-constructor

/** Custom initialization and start grabbing constructor. */
CDUO3DCamera::CDUO3DCamera(const TCaptureOptions_DUO3D& options)
    : m_duo(nullptr)
{
#if MRPT_HAS_DUO3D
    m_duo = new DUOInstance[1];
    M_DUO_VALUE = nullptr;  // m_duo = nullptr;

    m_pframe_data = nullptr;
    m_evFrame = CreateEvent(nullptr, FALSE, FALSE, nullptr);
    this->open(options);
#else
    THROW_EXCEPTION(
        "MRPT has been compiled with 'MRPT_BUILD_DUO3D'=OFF, so this class "
        "cannot be used.");
#endif
}  // end-constructor

/** Destructor */
CDUO3DCamera::~CDUO3DCamera()
{
#if MRPT_HAS_DUO3D
    this->close();
    if (m_duo)
    {
        delete[] M_DUO_PTR;
        m_duo = nullptr;
    }
#endif
}  // end-destructor

/** Tries to open the camera with the given options. Raises an exception on
 * error. \sa close() */
void CDUO3DCamera::open(
    const TCaptureOptions_DUO3D& options, const bool startCapture)
{
#if MRPT_HAS_DUO3D
    if (M_DUO_VALUE) this->close();
    this->m_options = options;

    if (this->m_options.m_calibration_from_file)
    {
        // get intrinsic parameters
        TCaptureOptions_DUO3D::TYMLReadResult res =
            this->m_options.m_camera_int_params_from_yml();
        if (res == TCaptureOptions_DUO3D::yrr_EMPTY)
            cout << "[CDUO3DCamera] Warning: Some of the intrinsic params "
                    "could not be read (size=0). Check file content."
                 << endl;
        else if (res == TCaptureOptions_DUO3D::yrr_NAME_NON_CONSISTENT)
            cout << "[CDUO3DCamera] Warning: Intrinsic params filename is not "
                    "consistent with image size. Are you using the correct "
                    "calibration?. All params set to zero."
                 << endl;

        // get extrinsic parameters
        res = this->m_options.m_camera_ext_params_from_yml();
        if (res == TCaptureOptions_DUO3D::yrr_EMPTY)
            cout << "[CDUO3DCamera] Warning: Some of the extrinsic params "
                    "could not be read (size!=3x3). Check file content."
                 << endl;
        else if (res == TCaptureOptions_DUO3D::yrr_NAME_NON_CONSISTENT)
            cout << "[CDUO3DCamera] Warning: Extrinsic params filename is not "
                    "consistent with image size. Are you using the correct "
                    "calibration?. All params set to zero."
                 << endl;

        if (this->m_options.m_capture_rectified)
        {
            if (!this->m_options.m_rectify_map_filename.empty())
            {
                // read "rectify_map"
                res = this->m_options.m_rectify_map_from_yml();
                if (res == TCaptureOptions_DUO3D::yrr_EMPTY)
                    cout << "[CDUO3DCamera] Warning: Rectification map could "
                            "not be read (size==0). Check file content."
                         << endl;
                else if (res == TCaptureOptions_DUO3D::yrr_NAME_NON_CONSISTENT)
                    cout << "[CDUO3DCamera] Warning: Rectification map "
                            "filename is not consistent with image size. Are "
                            "you using the correct calibration?. Rectification "
                            "map set to zero."
                         << endl;

                this->m_options.m_capture_rectified =
                    res == TCaptureOptions_DUO3D::yrr_OK;

                // const size_t area =
                // this->m_options.dp.m_rectify_map_left_x.size().area();
                TDUOParams& dp = duo_params[&(this->m_options)];
                const size_t area = dp.m_rectify_map_left_x.size().area();
                vector<int16_t> v_left_x(area), v_right_x(area);
                vector<uint16_t> v_left_y(area), v_right_y(area);

                for (size_t k = 0; k < area; ++k)
                {
                    v_left_x[k] = dp.m_rectify_map_left_x.at<int16_t>(k);
                    v_left_y[k] = dp.m_rectify_map_left_y.at<uint16_t>(k);
                    v_right_x[k] = dp.m_rectify_map_right_x.at<int16_t>(k);
                    v_right_y[k] = dp.m_rectify_map_right_y.at<uint16_t>(k);

                    // v_left_x[k] =
                    // this->m_options.dp.m_rectify_map_left_x.at<int16_t>(k);
                    // v_left_y[k] =
                    // this->m_options.dp.m_rectify_map_left_y.at<uint16_t>(k);
                    // v_right_x[k] =
                    // this->m_options.dp.m_rectify_map_right_x.at<int16_t>(k);
                    // v_right_y[k] =
                    // this->m_options.dp.m_rectify_map_right_y.at<uint16_t>(k);
                }
                m_rectify_map.setFromCamParams(this->m_options.m_stereo_camera);
                // m_rectify_map.setRectifyMaps( v_left_x, v_left_y, v_right_x,
                // v_right_y );
            }
            else
            {
                cout << "[CDUO3DCamera] Warning: Calibration information is "
                        "set to be read from a file, but the file was not "
                        "specified. Unrectified images will be grabbed."
                     << endl;
            }
        }  // end-if
    }  // end-if
    else if (this->m_options.m_capture_rectified)
    {
        m_rectify_map.setFromCamParams(this->m_options.m_stereo_camera);
    }

    // Find optimal binning parameters for given (width, height)
    // This maximizes sensor imaging area for given resolution
    int binning = DUO_BIN_NONE;
    if (this->m_options.m_img_width <= 752 / 2) binning += DUO_BIN_HORIZONTAL2;
    if (this->m_options.m_img_height <= 480 / 4)
        binning += DUO_BIN_VERTICAL4;
    else if (this->m_options.m_img_height <= 480 / 2)
        binning += DUO_BIN_VERTICAL2;

    // Check if we support given resolution (width, height, binning, fps)
    DUOResolutionInfo ri;
    if (!EnumerateResolutions(
            &ri, 1, this->m_options.m_img_width, this->m_options.m_img_height,
            binning, this->m_options.m_fps))
        THROW_EXCEPTION("[CDUO3DCamera] Error: Resolution not supported.");

    if (!OpenDUO(&M_DUO_VALUE))  // was: m_duo
        THROW_EXCEPTION("[CDUO3DCamera] Error: Camera could not be opened.");

    // Get and print some DUO parameter values
    char name[260], version[260];
    GetDUODeviceName(M_DUO_VALUE, name);
    GetDUOFirmwareVersion(M_DUO_VALUE, version);
    cout << "[CDUO3DCamera::open] DUO3DCamera name: " << name << " (v"
         << version << ")" << endl;

    // Set selected resolution
    SetDUOResolutionInfo(M_DUO_VALUE, ri);

    // Set selected camera settings
    SetDUOExposure(M_DUO_VALUE, m_options.m_exposure);
    SetDUOGain(M_DUO_VALUE, m_options.m_gain);
    SetDUOLedPWM(M_DUO_VALUE, m_options.m_led);

    // Start capture
    if (startCapture)
    {
        if (!StartDUO(M_DUO_VALUE, DUOCallback, reinterpret_cast<void*>(this)))
            THROW_EXCEPTION(
                "[CDUO3DCamera] Error: Camera could not be started.")
    }

#endif
}  // end-open

/*-------------------------------------------------------------
                        getObservations
-------------------------------------------------------------*/
void CDUO3DCamera::getObservations(
    CObservationStereoImages& outObservation_img,
    CObservationIMU& outObservation_imu, bool& there_is_img, bool& there_is_imu)
{
#if MRPT_HAS_DUO3D
    there_is_img = false;
    there_is_imu = false;

    m_pframe_data = m_get_duo_frame();
    if (!m_pframe_data) return;

    // -----------------------------------------------
    //   Extract the observation:
    // -----------------------------------------------
    outObservation_img.timestamp = outObservation_imu.timestamp =
        mrpt::system::now();

    outObservation_img.setStereoCameraParams(m_options.m_stereo_camera);
    outObservation_img.imageLeft.loadFromMemoryBuffer(
        m_options.m_img_width, m_options.m_img_height, false,
        (unsigned char*)reinterpret_cast<PDUOFrame>(m_pframe_data)->leftData);

    outObservation_img.imageRight.loadFromMemoryBuffer(
        m_options.m_img_width, m_options.m_img_height, false,
        (unsigned char*)reinterpret_cast<PDUOFrame>(m_pframe_data)->rightData);

    if (this->m_options.m_capture_rectified)
        m_rectify_map.rectify(outObservation_img);

    there_is_img = true;

    if (this->m_options.m_capture_imu)
    {
        if (!reinterpret_cast<PDUOFrame>(m_pframe_data)->accelerometerPresent)
        {
            cout << "[CDUO3DCamera] Warning: This device does not provide IMU "
                    "data. No IMU observations will be created."
                 << endl;
            this->m_options.m_capture_imu = false;
        }
        else
        {
            // Accelerometer data
            for (size_t k = 0; k < 3; ++k)
            {
                outObservation_imu.rawMeasurements[k] =
                    reinterpret_cast<PDUOFrame>(m_pframe_data)->accelData[k];
                outObservation_imu.dataIsPresent[k] = true;
            }

            // Gyroscopes data
            for (size_t k = 0; k < 3; ++k)
            {
                outObservation_imu.rawMeasurements[k + 3] =
                    reinterpret_cast<PDUOFrame>(m_pframe_data)->gyroData[k];
                outObservation_imu.dataIsPresent[k + 3] = true;
            }
            there_is_imu = true;
        }  // end else
    }  // end-imu-info
#endif
}

/** Closes DUO camera */
void CDUO3DCamera::close()
{
#if MRPT_HAS_DUO3D
    if (!M_DUO_VALUE) return;
    StopDUO(M_DUO_VALUE);
    CloseDUO(M_DUO_VALUE);
    M_DUO_VALUE = nullptr;
#endif
}  // end-close

// Waits until the new DUO frame is ready and returns it
void* CDUO3DCamera::m_get_duo_frame()
{
#if MRPT_HAS_DUO3D
    if (M_DUO_VALUE == nullptr) return 0;
    if (WaitForSingleObject(m_evFrame, 1000) == WAIT_OBJECT_0)
        return m_pframe_data;
    else
        return nullptr;
#else
    return nullptr;  // return something to silent compiler warnings.
#endif
}

void CDUO3DCamera::m_set_exposure(float value)
{
#if MRPT_HAS_DUO3D
    if (M_DUO_VALUE == nullptr) return;
    SetDUOExposure(M_DUO_VALUE, value);
#endif
}

void CDUO3DCamera::m_set_gain(float value)
{
#if MRPT_HAS_DUO3D
    if (M_DUO_VALUE == nullptr) return;
    SetDUOGain(M_DUO_VALUE, value);
#endif
}

void CDUO3DCamera::m_set_led(float value)
{
#if MRPT_HAS_DUO3D
    if (M_DUO_VALUE == nullptr) return;
    SetDUOLedPWM(M_DUO_VALUE, value);
#endif
}

/** Queries the DUO3D Camera firmware version */
bool CDUO3DCamera::queryVersion(std::string version, bool printOutVersion)
{
#if MRPT_HAS_DUO3D
    version = std::string(GetLibVersion());
    if (printOutVersion)
        std::cout << "DUO3D Camera library version: " << version << std::endl;
    return true;
#else
    return false;
#endif
}