Program Listing for File Frame.h

↰ Return to documentation for file (lib/DataStructures/Frame.h)

#pragma once
#include <mutex>
#include "util/SophusUtil.h"
#include "util/settings.h"
#include <boost/thread/recursive_mutex.hpp>
#include <boost/thread/shared_mutex.hpp>
#include "DataStructures/FramePoseStruct.h"
#include "DataStructures/FrameMemory.h"
#include "unordered_set"
#include "util/settings.h"
#include "util/Configuration.h"

#include "FrameData.h"

namespace lsd_slam
{

using libvideoio::Camera;
using libvideoio::ImageSize;

class KeyFrame;
class DepthMapPixelHypothesis;
class DepthMap;

template< int __LEVELS > class _TrackingRef;
typedef _TrackingRef<PYRAMID_LEVELS> TrackingReference;


class Frame
{
private:
    // Again, having wierd FrameData alignment issues where it's of different
    // lengths in different subunits unless it's at the top...
    FrameData<PYRAMID_LEVELS> data;

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    friend class FrameMemory;

    typedef std::shared_ptr<Frame> SharedPtr;

    // Explicitly delete default and copy constructors
    Frame() = delete;
    Frame( const Frame & ) = delete;

    Frame(int id, const Camera &cam, const ImageSize &sz, double timestamp, const unsigned char* image );
    Frame(int id, const Camera &cam, const ImageSize &sz, double timestamp, const float* image );

    ~Frame();

    void setDepth(const std::shared_ptr<DepthMap> &depthMap ); //PixelHypothesis* newDepth);

    void calculateMeanInformation();

    void setDepthFromGroundTruth(const float* depth, float cov_scale = 1.0f);

    void prepareForStereoWith( const Frame::SharedPtr &other, Sim3 thisToOther, const int level);



    // Accessors
    inline int id() const { return _id; }

    #define DATA_LEVEL_READER( _rtype, _name ) \
        inline _rtype _name( int level = 0 ) const \
            { return data._name[level]; }

    #define DATA_LEVEL_CAMERA_READER( _rtype, _name ) \
        inline _rtype _name( int level = 0 ) const \
            { return data.camera[level]._name; }

    inline const ImageSize &imgSize( int i=0 ) const    { return data.imgSize[i]; }
    inline int width( int i=0 ) const                                   { return data.imgSize[i].width; }
    inline int height( int i=0 ) const                              { return data.imgSize[i].height; }
    inline int area( int i=0 ) const                                    { return data.imgSize[i].area(); }


    DATA_LEVEL_READER( const Camera &, camera )

    DATA_LEVEL_CAMERA_READER( const Eigen::Matrix3f&, K )
    DATA_LEVEL_CAMERA_READER( const Eigen::Matrix3f&, Kinv )
    DATA_LEVEL_CAMERA_READER( float, fx )
    DATA_LEVEL_CAMERA_READER( float, fy )
    DATA_LEVEL_CAMERA_READER( float, cx )
    DATA_LEVEL_CAMERA_READER( float, cy )
    DATA_LEVEL_CAMERA_READER( float, fxi )
    DATA_LEVEL_CAMERA_READER( float, fyi )
    DATA_LEVEL_CAMERA_READER( float, cxi )
    DATA_LEVEL_CAMERA_READER( float, cyi )


    inline double timestamp() const { return _timestamp; }

    inline float* image(int level = 0);
    inline const Eigen::Vector4f* gradients(int level = 0);
    inline const float* maxGradients(int level = 0);
    inline bool hasIDepthBeenSet() const;
    inline const float* idepth(int level = 0);
    inline const float* idepthVar(int level = 0);
    inline const unsigned char* validity_reAct();
    inline const float* idepth_reAct();
    inline const float* idepthVar_reAct();

    inline bool* refPixelWasGood();
    inline bool* refPixelWasGoodNoCreate();
    inline void  clear_refPixelWasGood();

    enum DataFlags
    {
        IMAGE           = 1<<0,
        GRADIENTS       = 1<<1,
        MAX_GRADIENTS   = 1<<2,
        IDEPTH          = 1<<3,
        IDEPTH_VAR      = 1<<4,
        REF_ID          = 1<<5,

        ALL = IMAGE | GRADIENTS | MAX_GRADIENTS | IDEPTH | IDEPTH_VAR | REF_ID
    };


  // For SLAM-like features, KeyFrames can own their own TrackingReference
    // this is copied into the keyframe when the keyframe is finalized
    // This used for loop closure and re-localization
    // void setPermaRef( const std::unique_ptr<TrackingReference> &reference);
    // void takeReActivationData(DepthMapPixelHypothesis* depthMap);


    // shared_lock this as long as any minimizable arrays are being used.
    // the minimizer will only minimize frames after getting
    // an exclusive lock on this.
    inline boost::shared_lock<boost::shared_mutex> getActiveLock()
    {
        return FrameMemory::getInstance().activateFrame(this);
    }

    std::mutex frameMutex;


    /*
     * ==================================================================================
     * Here are ALL central pose and scale informations.
     * generally, everything is stored relative to the frame
     */
    FramePoseStruct::SharedPtr pose;
    Sim3 getCamToWorld()  { return pose->getCamToWorld(); }


    // parent, the frame originally tracked on. never changes.
    void setTrackingParent( const std::shared_ptr<KeyFrame> &newParent  ) { _trackingParent = newParent; }
    bool      hasTrackingParent() const                                                                 { return (bool)_trackingParent; }
    const std::shared_ptr<KeyFrame> &trackingParent() const                     { return _trackingParent; }

    bool isTrackingParent( const std::shared_ptr<Frame> &other ) const;
    bool isTrackingParent( const std::shared_ptr<KeyFrame> &other ) const;
    bool isTrackingParent( int id ) const;


    Sim3 lastConstraintTrackedCamToWorld;

    // flag set when depth is updated.
    //bool depthHasBeenUpdatedFlag;


    // Tracking Reference for quick test. Always available, never taken out of memory.
    // this is used for re-localization and re-Keyframe positioning.
    // boost::mutex permaRef_mutex;
    // Eigen::Vector3f* permaRef_posData;   // (x,y,z)
    // Eigen::Vector2f* permaRef_colorAndVarData;   // (I, Var)
    // int permaRefNumPts;



    // A bunch of state which is created by prepareForStereoWith()
    int referenceID;
    int referenceLevel;
    float distSquared;
    Eigen::Matrix3f K_otherToThis_R;
    Eigen::Vector3f K_otherToThis_t;
    Eigen::Vector3f otherToThis_t;
    Eigen::Vector3f K_thisToOther_t;
    Eigen::Matrix3f thisToOther_R;
    Eigen::Vector3f otherToThis_R_row0;
    Eigen::Vector3f otherToThis_R_row1;
    Eigen::Vector3f otherToThis_R_row2;
    Eigen::Vector3f thisToOther_t;



    // statistics
    float initialTrackedResidual;
    float meanIdepth;
    int numPoints;
    int idxInKeyframes;
    float edgeErrorSum, edgesNum;
    int numMappablePixels;
    float meanInformation;

private:

    std::shared_ptr<KeyFrame> _trackingParent;

    int _id;
    double _timestamp;

    void require(int dataFlags, int level = 0);
    void release(int dataFlags, bool pyramidsOnly, bool invalidateOnly);

//  void initialize(double timestamp);
    void setDepth_Allocate();

    void buildImage(int level);
    void releaseImage(int level);

    void buildGradients(int level);
    void releaseGradients(int level);

    void buildMaxGradients(int level);
    void releaseMaxGradients(int level);

    void buildIDepthAndIDepthVar(int level);
    void releaseIDepth(int level);
    void releaseIDepthVar(int level);

    // used internally. locked while something is being built, such that no
    // two threads build anything simultaneously. not locked on require() if nothing is changed.
    boost::mutex buildMutex;

    boost::shared_mutex activeMutex;
    bool isActive;

    bool minimizeInMemory();

};


inline float* Frame::image(int level)
{
    if (! data.imageValid[level])
        require(IMAGE, level);
    return data.image[level];
}

inline const Eigen::Vector4f* Frame::gradients(int level)
{
    if (! data.gradientsValid[level])
        require(GRADIENTS, level);
    return data.gradients[level];
}

inline const float* Frame::maxGradients(int level)
{
    if (! data.maxGradientsValid[level])
        require(MAX_GRADIENTS, level);
    return data.maxGradients[level];
}

inline bool Frame::hasIDepthBeenSet() const
{
    return data.hasIDepthBeenSet;
}

inline const float* Frame::idepth(int level)
{
    if (! hasIDepthBeenSet())
    {
        LOG(WARNING) << "Frame " <<id() << "; idepth(): idepth has not been set yet!";
        return nullptr;
    }
    if (! data.idepthValid[level])
        require(IDEPTH, level);
    return data.idepth[level];
}

inline const unsigned char* Frame::validity_reAct()
{
    if( !data.reActivationDataValid)
        return 0;
    return data.validity_reAct;
}

inline const float* Frame::idepth_reAct()
{
    if( !data.reActivationDataValid)
        return 0;
    return data.idepth_reAct;
}

inline const float* Frame::idepthVar_reAct()
{
    if( !data.reActivationDataValid)
        return 0;
    return data.idepthVar_reAct;
}

inline const float* Frame::idepthVar(int level)
{
    if (! hasIDepthBeenSet())
    {
        LOG(WARNING) << "Frame " << id() << "; idepthVar(): idepth has not been set yet!";
        return nullptr;
    }
    if (! data.idepthVarValid[level]) require(IDEPTH_VAR, level);
    return data.idepthVar[level];
}


inline bool* Frame::refPixelWasGood()
{
    if( data.refPixelWasGood == 0)
    {
        boost::unique_lock<boost::mutex> lock2(buildMutex);

        if(data.refPixelWasGood == 0)
        {
            const int width = data.imgSize[SE3TRACKING_MIN_LEVEL].width;
            const int height = data.imgSize[SE3TRACKING_MIN_LEVEL].height;
            data.refPixelWasGood = (bool*)FrameMemory::getInstance().getBuffer(sizeof(bool) * width * height);

            memset(data.refPixelWasGood, 0xFFFFFFFF, sizeof(bool) * (width * height));
        }
    }
    return data.refPixelWasGood;
}


inline bool* Frame::refPixelWasGoodNoCreate()
{
    return data.refPixelWasGood;
}

inline void Frame::clear_refPixelWasGood()
{
    FrameMemory::getInstance().returnBuffer(reinterpret_cast<float*>(data.refPixelWasGood));
    data.refPixelWasGood=0;
}


}