Program Listing for File Frame.h

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

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

#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;
  cv::Mat getCvImage();
  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;
}

} // namespace lsd_slam