CStereoGrabber_SVS.cpp

Go to the documentation of this file.

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

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

#include <mrpt/hwdrivers/CStereoGrabber_SVS.h>

// Universal include for all versions of OpenCV
#include <mrpt/3rdparty/do_opencv_includes.h>

#if MRPT_HAS_SVS
#include <dcs.h>
#include <svsclass.h>
#endif

using namespace std;
using namespace mrpt;
using namespace mrpt::obs;
using namespace mrpt::hwdrivers;

/*-------------------------------------------------------------
                    Constructor
 -------------------------------------------------------------*/
CStereoGrabber_SVS::CStereoGrabber_SVS(
    int cameraIndex, const TCaptureOptions_SVS& options)
    : m_resolutionX(options.frame_width),
      m_resolutionY(options.frame_height),
      m_procesOnChip(options.m_procesOnChip),
      m_calDisparity(options.m_calDisparity),
      m_options(options)
{
#if MRPT_HAS_SVS

    // get the svsVideoImages object from the currently loaded camera interface
    m_videoObject = static_cast<svsVideoImages*>(getVideoObject());
    cout << "Using live images:" << endl;
    cout << "svsVideoIdent" << endl;

    // Open the stereo device
    bool ret;
    ret = static_cast<svsVideoImages*>(m_videoObject)->Open();
    if (ret)
    {
        cout << " stereo device Opened" << endl;

        static_cast<svsVideoImages*>(m_videoObject)
            ->SetSize(m_resolutionX, m_resolutionY);  // width x height image

        m_disparityParams =
            static_cast<svsVideoImages*>(m_videoObject)->GetDP();
        static_cast<svsVideoImages*>(m_videoObject)
            ->SetNDisp(m_options.m_NDisp);  // 32 disparities
        static_cast<svsVideoImages*>(m_videoObject)
            ->SetCorrsize(m_options.m_Corrsize);  // correlation window size
        static_cast<svsVideoImages*>(m_videoObject)
            ->SetLR(m_options.m_LR);  // no left-right check, not available
        static_cast<svsVideoImages*>(m_videoObject)
            ->SetThresh(m_options.m_Thresh);  // texture filter
        static_cast<svsVideoImages*>(m_videoObject)
            ->SetUnique(m_options.m_Unique);  // uniqueness filter
        static_cast<svsVideoImages*>(m_videoObject)
            ->SetHoropter(m_options.m_Horopter);  // horopter offset

        if (!(static_cast<svsVideoImages*>(m_videoObject)
                  ->SetExposure(0, 0, true, true)))
        {
            cout << "Can't set Auto exposure" << endl;
        }
        else
        {
            cout << "Autoexposure set to 0 0" << endl;
        }

        /*  videoObject->SetBrightness(true, 0);
           videoObject->SetAutoExpParams(0.0, -2.0);
       */
        /// videoObject->SetGamma(); search for auto gamma ?

        static_cast<svsVideoImages*>(m_videoObject)
            ->SetRate(m_options.framerate);
        static_cast<svsVideoImages*>(m_videoObject)
            ->SetSpeckleSize(m_options.m_SpeckleSize);  // TODO add in config

        // TODO call CheckParam
        if (static_cast<svsVideoImages*>(m_videoObject)->CheckParams())
        {
            cout << "Params OK !" << endl;
            m_initialized = true;
            m_status = true;
            bool ret = static_cast<svsVideoImages*>(m_videoObject)->Start();
            if (ret)
            {
                cout << " Start Continuous mode" << endl;

                // NOTE: to do rectification, we have to turn it on...
                // Here we optionally set up acquisition to rectify the image

                ret =
                    static_cast<svsVideoImages*>(m_videoObject)->SetRect(true);
                if (ret)
                {
                    cout << "Images will be rectified" << endl;
                }
                else
                {
                    cout << "Can't set rectification" << endl;
                }
                if (m_procesOnChip)
                {
                    // NOTE: for STOC device, turn on stereo processing on-chip
                    if (static_cast<svsVideoImages*>(m_videoObject) &&
                        static_cast<svsVideoImages*>(m_videoObject)
                            ->is_proc_capable)  // can we process on-camera?
                    {
                        static_cast<svsVideoImages*>(m_videoObject)
                            ->SetProcMode(PROC_MODE_DISPARITY);
                        cout << "Setting STOC disparity mode" << endl;
                    }
                }
                else
                {
                    if (m_processObject)
                        m_processObject = new svsStereoProcess();
                    // NOTE: for STOC device, turn off stereo processing on-chip
                    if (static_cast<svsVideoImages*>(m_videoObject) &&
                        static_cast<svsVideoImages*>(m_videoObject)
                            ->is_proc_capable)  // can we process on-camera?
                    {
                        static_cast<svsVideoImages*>(m_videoObject)
                            ->SetProcMode(PROC_MODE_OFF);
                        cout << "Setting STOC stereo mode" << endl;
                    }
                }
            }
            else
            {
                cout << "Can't start continuous capture" << endl;
                m_status = false;
            }
        }
        else
        {
            m_initialized = false;
            m_status = false;
            cout << "Params Unconsistents !" << endl;
        }
    }
    else

        // TODO essayer de faire un close...
        cout << "Can't open stereo device" << endl;

    m_status = false;

#else
    MRPT_UNUSED_PARAM(cameraIndex);
    THROW_EXCEPTION("This class requires MRPT built with Videre SVS library.");
#endif
}

/*-------------------------------------------------------------
                    Destructor
 -------------------------------------------------------------*/
CStereoGrabber_SVS::~CStereoGrabber_SVS()
{
#if MRPT_HAS_SVS
    static_cast<svsVideoImages*>(m_videoObject)->Close();
#endif  // No need to raise an exception on "#else" since it's already raised
    // upon construction.
}

/*-------------------------------------------------------------
                    get the image
 -------------------------------------------------------------*/
bool CStereoGrabber_SVS::getStereoObservation(
    mrpt::obs::CObservationStereoImages& out_observation)
{
#if MRPT_HAS_SVS
    if ((m_stereoImage =
             static_cast<svsVideoImages*>(m_videoObject)->GetImage(500)) &&
        static_cast<svsStereoImage*>(m_stereoImage)->haveImages)  // 500 ms
    // timeout
    // //TODO
    // adjust
    // timeout
    // with
    // framerate
    {
        // get disparity params
        m_disparityParams =
            static_cast<svsVideoImages*>(m_videoObject)->GetDP();

        const size_t sizeOfMat = m_resolutionX * m_resolutionY;

        IplImage* ImageLeft = cvCreateImageHeader(
            cvSize(m_resolutionX, m_resolutionY), IPL_DEPTH_8U, 1);

        ImageLeft->widthStep =
            ImageLeft->width;  // JL: The next line assumes this
        ImageLeft->imageData =
            (char*)static_cast<svsStereoImage*>(m_stereoImage)->Left();

        if (m_procesOnChip)
        {
            IplImage* ImageDisparity = cvCreateImage(
                cvSize(m_resolutionX, m_resolutionY), IPL_DEPTH_8U, 1);

            unsigned char* ptrOutDisp;
            short int* ptrDisp;

            ptrDisp = static_cast<svsStereoImage*>(m_stereoImage)->Disparity();
            ptrOutDisp = (unsigned char*)ImageDisparity->imageData;

            ASSERT_(
                ImageDisparity->widthStep ==
                ImageDisparity->width);  // JL: The code below assumes
            // image_width == widthStep

            for (int pix = 0; pix < sizeOfMat; pix++, ptrOutDisp++, ptrDisp++)
            {
                if (*(ptrDisp) > 0)
                    *(ptrOutDisp) = (unsigned char)((*(ptrDisp) >> 2) & 0x00FF);
                else
                    *(ptrOutDisp) = 0;
            }

            // Create the object to be return (it will have a fresh timestamp if
            // it's created now and here):
            CObservationStereoImages ret_obj(
                cvCloneImage(ImageLeft),  // Create a new IplImage* which will
                // be owned by the observation object.
                nullptr /*has no right*/, ImageDisparity,
                true /* own the memory, so we don't have to free it here */);

            out_observation.swap(
                ret_obj);  // Send as output (faster than a "=").
        }
        else if (m_calDisparity)
        {
            static_cast<svsStereoProcess*>(m_processObject)
                ->CalcStereo(static_cast<svsStereoImage*>(m_stereoImage));
            IplImage* ImageDisparity = cvCreateImage(
                cvSize(m_resolutionX, m_resolutionY), IPL_DEPTH_8U, 1);

            unsigned char* ptrOutDisp;
            short int* ptrDisp;

            ptrDisp = static_cast<svsStereoImage*>(m_stereoImage)->Disparity();
            ptrOutDisp = (unsigned char*)ImageDisparity->imageData;

            ASSERT_(
                ImageDisparity->widthStep ==
                ImageDisparity->width);  // JL: The code below assumes
            // image_width == widthStep

            for (int pix = 0; pix < sizeOfMat; pix++, ptrOutDisp++, ptrDisp++)
            {
                if (*(ptrDisp) > 0)
                    *(ptrOutDisp) = (unsigned char)((*(ptrDisp) >> 2) & 0x00FF);
                else
                    *(ptrOutDisp) = 0;
            }

            // Create the object to be return (it will have a fresh timestamp if
            // it's created now and here):
            CObservationStereoImages ret_obj(
                cvCloneImage(ImageLeft),  // Create a new IplImage* which will
                // be owned by the observation object.
                nullptr /*has no right*/, ImageDisparity,
                true /* own the memory, so we don't have to free it here */);

            out_observation.swap(
                ret_obj);  // Send as output (faster than a "=").
        }
        else
        {
            IplImage* ImageRight = cvCreateImageHeader(
                cvSize(m_resolutionX, m_resolutionY), IPL_DEPTH_8U, 1);
            ImageRight->widthStep =
                ImageRight->width;  // JL: The next line assumes this
            ImageRight->imageData =
                (char*)static_cast<svsStereoImage*>(m_stereoImage)->Right();

            // Create the object to be return (it will have a fresh timestamp if
            // it's created now and here):
            CObservationStereoImages ret_obj(
                cvCloneImage(ImageLeft),  // Create a new IplImage* which will
                // be owned by the observation object.
                cvCloneImage(ImageRight), nullptr /*has no disparity*/,
                true /* own the memory, so we don't have to free it here */);

            out_observation.swap(
                ret_obj);  // Send as output (faster than a "=").
        }

        // cvReleaseImage(&Image Disparity); // No need anymore to release
        // images...
        return true;
    }

    return false;
// All ok
#else
    MRPT_UNUSED_PARAM(out_observation);
    // No need to raise an exception on "#else" since it's already raised upon
    // construction.
    return false;  // This shouldn't actually be never reached, just to please
// the compiler.
#endif
}

/*-------------------------------------------------------------
                        TCaptureOptions_bumblebee Constructor
 -------------------------------------------------------------*/
TCaptureOptions_SVS::TCaptureOptions_SVS(
    int _frame_width, int _frame_height, double _framerate, int _NDisp,
    int _Corrsize, int _LR, int _Thresh, int _Unique, int _Horopter,
    int _SpeckleSize, bool _procesOnChip, bool _calDisparity)
{
    frame_width = _frame_width;
    frame_height = _frame_height;
    framerate = _framerate;
    m_NDisp = _NDisp;  // 32 disparities
    m_Corrsize = _Corrsize;  // correlation window size
    m_LR = _LR;  // no left-right check, not available
    m_Thresh = _Thresh;  // texture filter
    m_Unique = _Unique;  // uniqueness filter
    m_Horopter = _Horopter;
    m_SpeckleSize = _SpeckleSize;
    m_procesOnChip = _procesOnChip;
    m_calDisparity = _calDisparity;
}