COpenNI2Generic_CDevice.h

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          |
   +------------------------------------------------------------------------+ */

#pragma once

#include <mrpt/config.h>  //MRPT_HAS_OPENNI2

/** @file Include this file only from your user code if you have OPENNI2 */

namespace mrpt::hwdrivers
{
class COpenNI2Generic::CDevice
{
   public:
    using Ptr = std::shared_ptr<CDevice>;
    enum
    {
        COLOR_STREAM,
        DEPTH_STREAM,
        IR_STREAM,
        STREAM_TYPE_SIZE  // this last value is to know the number of possible
        // channels, leave it always at the end!
    };
#if MRPT_HAS_OPENNI2
   private:
    class CStream
    {
       public:
        using Ptr = std::shared_ptr<CStream>;

       private:
        std::ostream& m_log;
        openni::Device& m_device;
        std::string m_strName;
        openni::SensorType m_type;
        openni::VideoStream m_stream;
        openni::PixelFormat m_format;
        bool m_verbose;

       public:
        CStream(
            openni::Device& device, openni::SensorType type,
            openni::PixelFormat format, std::ostream& log, bool verbose);
        virtual ~CStream();
        const std::string& getName() const { return m_strName; }
        bool isValid() const;
        bool isMirrorSupported() const;
        bool setMirror(bool flag);
        void setCloseRange(int& value);
        virtual bool open(int w, int h, int fps);
        virtual bool start();
        virtual void destroy();
        virtual bool getFrame(
            openni::VideoFrameRef& frame, mrpt::system::TTimeStamp& timestamp,
            bool& there_is_obs, bool& hardware_error);

        int getFrameWidth() const
        {
            return m_stream.getVideoMode().getResolutionX();
        }
        int getFrameHeight() const
        {
            return m_stream.getVideoMode().getResolutionY();
        }
        double getHFov() const { return m_stream.getHorizontalFieldOfView(); }
        double getFx() const
        {
            return getFrameWidth() / (2.0 * tan(getHFov() / 2.0));
        }
        double getVFov() const { return m_stream.getVerticalFieldOfView(); }
        double getFy() const
        {
            return getFrameHeight() / (2.0 * tan(getVFov() / 2.0));
        }
        double getCx() const { return (getFrameWidth() - 1) * 0.5; }
        double getCy() const { return (getFrameHeight() - 1) * 0.5; }
        void disableAutoExposure()
        {
            m_stream.getCameraSettings()->setAutoExposureEnabled(false);
        }
        void enableAutoExposure()
        {
            m_stream.getCameraSettings()->setAutoExposureEnabled(true);
        }

        void getCameraParam(mrpt::img::TCamera& param) const
        {
            param.ncols = getFrameWidth();
            param.nrows = getFrameHeight();
            param.fx(getFx());
            param.fy(getFy());
            param.cx(getCx());
            param.cy(getCy());
        }

        static Ptr create(
            openni::Device& device, openni::SensorType type,
            openni::PixelFormat format, std::ostream& log, bool verbose);
    };
    openni::DeviceInfo m_info;
    openni::Device m_device;
    CStream::Ptr m_streams[STREAM_TYPE_SIZE];
    bool m_mirror;
    std::stringstream m_log;
    bool m_verbose;

    bool synchMirrorMode();
    bool startStreams();

    inline void resize(mrpt::img::CImage& rgb, int w, int h)
    {
        rgb.resize(w, h, mrpt::img::CH_RGB);
    }
    inline void resize(mrpt::math::CMatrix_u16& depth, int w, int h)
    {
        depth.resize(h, w);
    }
    inline void resize(mrpt::obs::CObservation3DRangeScan& obs, int w, int h)
    {
        resize(obs.intensityImage, w, h);
        obs.rangeImage_setSize(h, w);
    }

    inline void setPixel(
        const openni::RGB888Pixel& src, mrpt::img::CImage& rgb, int x, int y)
    {
        rgb.setPixel(x, y, (src.r << 16) + (src.g << 8) + src.b);
    }
    inline void setPixel(
        const openni::DepthPixel& src, mrpt::math::CMatrix_u16& depth_mm, int x,
        int y)
    {
        depth_mm(y, x) = src;
    }

    template <class NI_PIXEL, class MRPT_DATA>
    void copyRow(const char* src, MRPT_DATA& rgb, int w, const int y)
    {
        const auto* s = (const NI_PIXEL*)src;
        for (int xc = 0; xc < w; ++xc, ++s)
        {
            int x = xc;
            if (isMirrorMode())
            {
                x = w - xc - 1;
            }
            setPixel(*s, rgb, x, y);
        }
    }

    template <class NI_PIXEL, class MRPT_DATA>
    void copyFrame(openni::VideoFrameRef& frame, MRPT_DATA& dst)
    {
        const char* data = (const char*)frame.getData();
        const int stride = frame.getStrideInBytes();
        const int width = frame.getWidth();
        const int height = frame.getHeight();
        resize(dst, width, height);
        for (int y = 0; y < height; ++y, data += stride)
        {
            copyRow<NI_PIXEL, MRPT_DATA>(data, dst, width, y);
        }
    }

   public:
    CDevice(
        const openni::DeviceInfo& info, openni::PixelFormat rgb,
        openni::PixelFormat depth, bool m_verbose);
    virtual ~CDevice();

    const openni::DeviceInfo& getInfo() const { return m_info; }
    std::string getLog() const { return m_log.str(); }
    void clearLog()
    {
        m_log.str("");
        m_log.clear();
    }
    bool isMirrorMode() const { return m_mirror; }
    void setMirrorMode(bool mode) { m_mirror = mode; }
    bool hasColor() const
    {
        if (!m_streams[COLOR_STREAM])
            return false;
        else
            return m_streams[COLOR_STREAM]->isValid();
    }
    bool hasDepth() const
    {
        if (!m_streams[DEPTH_STREAM])
            return false;
        else
            return m_streams[DEPTH_STREAM]->isValid();
    }

    bool isOpen() const;
    void close();
    bool open(int w, int h, int fps);

    bool getNextFrameRGB(
        mrpt::img::CImage& img, mrpt::system::TTimeStamp& timestamp,
        bool& there_is_obs, bool& hardware_error);
    bool getNextFrameD(
        mrpt::math::CMatrix_u16& depth_mm, mrpt::system::TTimeStamp& timestamp,
        bool& there_is_obs, bool& hardware_error);
    bool getNextFrameRGBD(
        mrpt::obs::CObservation3DRangeScan& obs, bool& there_is_obs,
        bool& hardware_error);

    bool getCameraParam(int streamType, mrpt::img::TCamera& param) const
    {
        if (streamType < 0 || streamType >= STREAM_TYPE_SIZE)
        {
            return false;
        }
        if (!m_streams[streamType] || m_streams[streamType]->isValid() == false)
        {
            return false;
        }
        m_streams[streamType]->getCameraParam(param);
        return true;
    }

    bool getSerialNumber(unsigned int& sn);

    static Ptr create(
        const openni::DeviceInfo& info, openni::PixelFormat rgb,
        openni::PixelFormat depth, bool verbose);

    openni::Device& getDevicePtr() { return m_device; }

   private:
    bool getSerialNumber(std::string& sn);

#endif  // MRPT_HAS_OPENNI2
};
}  // namespace mrpt::hwdrivers