CStereoGrabber_SVS.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/hwdrivers/CStereoGrabber_SVS.h>

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

#if MRPT_HAS_SVS
#include <svsclass.h>
#include <dcs.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_bInitialized(false),
          m_videoObject(nullptr),
          m_stereoImage(nullptr),
          m_disparityParams(nullptr),
          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;
}