CDUO3DCamera.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, 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> 

// 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::utils;
using namespace mrpt::poses;
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 = CPose3DQuat();
                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 = CPose3DQuat();
        }

        // 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 = CPose3DQuat();
        }

        if( empty ) return yrr_EMPTY;

        m_stereo_camera.rightCameraPose = CPose3DQuat( CPose3D(M,t) );
        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.zeros();
                m_stereo_camera.rightCamera.intrinsicParams.zeros();
                m_stereo_camera.rightCamera.dist.zeros();

                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.zeros();
        }
        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.zeros();
        }
        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::utils::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(NULL)
{
#if MRPT_HAS_DUO3D
        m_duo = new DUOInstance[1];
        M_DUO_VALUE = NULL;  // m_duo = NULL;

        m_pframe_data = NULL;
        m_evFrame = CreateEvent(NULL, FALSE, FALSE, NULL);
#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(NULL)
{
#if MRPT_HAS_DUO3D
        m_duo = new DUOInstance[1];
        M_DUO_VALUE = NULL;  // m_duo = NULL;

        m_pframe_data = NULL;
        m_evFrame = CreateEvent(NULL, FALSE, FALSE, NULL);
        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=NULL;}
#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 = NULL;
#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 == NULL ) return 0;
        if(WaitForSingleObject( m_evFrame, 1000 ) == WAIT_OBJECT_0) return m_pframe_data;
        else return NULL;
#else
        return NULL; // return something to silent compiler warnings.
#endif
}

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

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

void CDUO3DCamera::m_set_led(float value)
{
#if MRPT_HAS_DUO3D
        if( M_DUO_VALUE == NULL ) 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
}