Program Listing for File visual-frame.h

Return to documentation for file (aslam_cv2/aslam_cv_frames/include/aslam/frames/visual-frame.h)

#ifndef ASLAM_FRAMES_VISUAL_FRAME_H_
#define ASLAM_FRAMES_VISUAL_FRAME_H_

#include <memory>
#include <unordered_map>
#include <cstdint>

#include <aslam/cameras/camera.h>
#include <aslam/common/channel.h>
#include <aslam/common/channel-declaration.h>
#include <aslam/common/macros.h>
#include <aslam/common/unique-id.h>
#include <Eigen/Dense>

namespace aslam {
class Camera;

class VisualFrame  {
 public:
  typedef Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic> DescriptorsT;
  typedef Eigen::VectorXd KeypointScoresT;

  ASLAM_POINTER_TYPEDEFS(VisualFrame);
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  VisualFrame();
  virtual ~VisualFrame() {};

  VisualFrame(const VisualFrame& other);
  VisualFrame& operator=(const VisualFrame& other);

  virtual bool operator==(const VisualFrame& other) const;

  virtual bool compareWithoutCameraGeometry(const VisualFrame& other) const;

  template<typename CHANNEL_DATA_TYPE>
  void addChannel(const std::string& channel) {
    aslam::channels::addChannel<CHANNEL_DATA_TYPE>(channel, &channels_);
  }

  bool hasKeypointMeasurements() const;

  bool hasKeypointMeasurementUncertainties() const;

  bool hasKeypointOrientations() const;

  bool hasKeypointScores() const;

  bool hasKeypointScales() const;

  bool hasDescriptors() const;

  bool hasTrackIds() const;

  bool hasRawImage() const;

  bool hasChannel(const std::string& channel) const {
    return aslam::channels::hasChannel(channel, channels_);
  }

  void clearKeypointChannels();

  const Eigen::Matrix2Xd& getKeypointMeasurements() const;

  inline size_t getNumKeypointMeasurements() const {
    return static_cast<size_t>(getKeypointMeasurements().cols());
  }

  const Eigen::VectorXd& getKeypointMeasurementUncertainties() const;

  const Eigen::VectorXd& getKeypointOrientations() const;

  const Eigen::VectorXd& getKeypointScores() const;

  const Eigen::VectorXd& getKeypointScales() const;

  const DescriptorsT& getDescriptors() const;

  const Eigen::VectorXi& getTrackIds() const;

  const cv::Mat& getRawImage() const;

  void releaseRawImage();

  template<typename CHANNEL_DATA_TYPE>
  const CHANNEL_DATA_TYPE& getChannelData(const std::string& channel) const {
    return aslam::channels::getChannelData<CHANNEL_DATA_TYPE>(channel, channels_);
  }

  Eigen::Matrix2Xd* getKeypointMeasurementsMutable();

  Eigen::VectorXd* getKeypointMeasurementUncertaintiesMutable();

  Eigen::VectorXd* getKeypointOrientationsMutable();

  Eigen::VectorXd* getKeypointScoresMutable();

  Eigen::VectorXd* getKeypointScalesMutable();

  DescriptorsT* getDescriptorsMutable();

  Eigen::VectorXi* getTrackIdsMutable();

  cv::Mat* getRawImageMutable();

  template<typename CHANNEL_DATA_TYPE>
  CHANNEL_DATA_TYPE* getChannelDataMutable(const std::string& channel) const {
    CHANNEL_DATA_TYPE& data =
        aslam::channels::getChannelData<CHANNEL_DATA_TYPE>(channel,
                                                           channels_);
    return &data;
  }

  const Eigen::Block<Eigen::Matrix2Xd, 2, 1> getKeypointMeasurement(size_t index) const;

  double getKeypointMeasurementUncertainty(size_t index) const;

  double getKeypointOrientation(size_t index) const;

  double getKeypointScore(size_t index) const;

  double getKeypointScale(size_t index) const;

  const unsigned char* getDescriptor(size_t index) const;

  int getTrackId(size_t index) const;

  void setKeypointMeasurements(const Eigen::Matrix2Xd& keypoints);

  void setKeypointMeasurementUncertainties(const Eigen::VectorXd& uncertainties);

  void setKeypointOrientations(const Eigen::VectorXd& orientations);

  void setKeypointScores(const Eigen::VectorXd& scores);

  void setKeypointScales(const Eigen::VectorXd& scales);

  void setDescriptors(const DescriptorsT& descriptors);

  void setDescriptors(const Eigen::Map<const DescriptorsT>& descriptors);

  void setTrackIds(const Eigen::VectorXi& track_ids);

  void setRawImage(const cv::Mat& image);

  template<typename CHANNEL_DATA_TYPE>
  void setChannelData(const std::string& channel,
                      const CHANNEL_DATA_TYPE& data_new) {
    if (!aslam::channels::hasChannel(channel, channels_)) {
      aslam::channels::addChannel<CHANNEL_DATA_TYPE>(channel, &channels_);
    }
    CHANNEL_DATA_TYPE& data =
        aslam::channels::getChannelData<CHANNEL_DATA_TYPE>(channel, channels_);
    data = data_new;
  }

  void swapKeypointMeasurements(Eigen::Matrix2Xd* keypoints);

  void swapKeypointMeasurementUncertainties(Eigen::VectorXd* uncertainties);

  void swapKeypointOrientations(Eigen::VectorXd* orientations);

  void swapKeypointScores(Eigen::VectorXd* scores);

  void swapKeypointScales(Eigen::VectorXd* scales);

  void swapDescriptors(DescriptorsT* descriptors);

  void swapTrackIds(Eigen::VectorXi* track_ids);

  template<typename CHANNEL_DATA_TYPE>
  void swapChannelData(const std::string& channel,
                       CHANNEL_DATA_TYPE* data_new) {
    CHECK_NOTNULL(data_new);
    if (!aslam::channels::hasChannel(channel, channels_)) {
      aslam::channels::addChannel<CHANNEL_DATA_TYPE>(channel, &channels_);
    }
    CHANNEL_DATA_TYPE& data =
        aslam::channels::getChannelData<CHANNEL_DATA_TYPE>(channel, channels_);
    data.swap(*data_new);
  }

  const Camera::ConstPtr getCameraGeometry() const;

  void setCameraGeometry(const Camera::ConstPtr& camera);

  const Camera::ConstPtr getRawCameraGeometry() const;

  void setRawCameraGeometry(const Camera::ConstPtr& camera);

  aslam::ProjectionResult getKeypointInRawImageCoordinates(size_t keypoint_idx,
      Eigen::Vector2d* keypoint_raw_coordinates) const;

  aslam::ProjectionResult toRawImageCoordinates(const Eigen::Vector2d& keypoint,
                                                Eigen::Vector2d* out_image_coordinates) const;

  void toRawImageCoordinatesVectorized(const Eigen::Matrix2Xd& keypoints,
                                       Eigen::Matrix2Xd* out_image_coordinates,
                                       std::vector<aslam::ProjectionResult>* results) const;

  Eigen::Matrix3Xd getNormalizedBearingVectors(
      const std::vector<size_t>& keypoint_indices,
      std::vector<unsigned char>* backprojection_success) const;

  inline const aslam::FrameId& getId() const { return id_; }

  inline void setId(const aslam::FrameId& id) { id_ = id; }

  inline int64_t getTimestampNanoseconds() const { return timestamp_nanoseconds_; }

  inline void setTimestampNanoseconds(int64_t timestamp_nanoseconds) {
    timestamp_nanoseconds_ = timestamp_nanoseconds;
  }

  size_t getDescriptorSizeBytes() const;

  void validate() { is_valid_ = true; }
  void invalidate(){ is_valid_ = false; }
  bool isValid() const { return is_valid_; }
  void setValid(bool is_valid) { is_valid_ = is_valid; }

  void print(std::ostream& out, const std::string& label) const;

  static VisualFrame::Ptr createEmptyTestVisualFrame(const aslam::Camera::ConstPtr& camera,
                                                     int64_t timestamp_nanoseconds);

  void discardUntrackedObservations(std::vector<size_t>* discarded_indices);

 private:
  int64_t timestamp_nanoseconds_;

  aslam::FrameId id_;
  aslam::channels::ChannelGroup channels_;
  Camera::ConstPtr camera_geometry_;
  Camera::ConstPtr raw_camera_geometry_;

  bool is_valid_;
};

inline std::ostream& operator<<(std::ostream& out, const VisualFrame& rhs) {
  rhs.print(out, "");
  return out;
}
}  // namespace aslam
#endif  // ASLAM_FRAMES_VISUAL_FRAME_H_