Program Listing for File camera.h

Return to documentation for file (aslam_cv2/aslam_cv_cameras/include/aslam/cameras/camera.h)

#ifndef ASLAM_CAMERAS_CAMERA_H_
#define ASLAM_CAMERAS_CAMERA_H_

#include <cstdint>
#include <iostream>
#include <memory>
#include <string>
#include <vector>

#include <Eigen/Dense>
#include <glog/logging.h>

#include <aslam/cameras/distortion-null.h>
#include <aslam/cameras/distortion.h>
#include <aslam/common/macros.h>
#include <aslam/common/sensor.h>
#include <aslam/common/types.h>
#include <aslam/common/unique-id.h>

// TODO(slynen) Enable commented out PropertyTree support
// namespace sm {
// class PropertyTree;
//}

namespace aslam {

// Forward declarations
class MappedUndistorter;

struct ProjectionResult {
  enum class Status {
    KEYPOINT_VISIBLE,
    KEYPOINT_OUTSIDE_IMAGE_BOX,
    POINT_BEHIND_CAMERA,
    PROJECTION_INVALID,
    UNINITIALIZED
  };
  // Make the enum values accessible from the outside without the additional
  // indirection.
  static Status KEYPOINT_VISIBLE;
  static Status KEYPOINT_OUTSIDE_IMAGE_BOX;
  static Status POINT_BEHIND_CAMERA;
  static Status PROJECTION_INVALID;
  static Status UNINITIALIZED;

  constexpr ProjectionResult() : status_(Status::UNINITIALIZED){};
  constexpr ProjectionResult(Status status) : status_(status){};

  explicit operator bool() const {
    return isKeypointVisible();
  };

  bool operator==(const ProjectionResult& other) const {
    return status_ == other.status_;
  };

  bool operator==(const ProjectionResult::Status& other) const {
    return status_ == other;
  };

  friend std::ostream& operator<<(
      std::ostream& out, const ProjectionResult& state);

  bool isKeypointVisible() const {
    return (status_ == Status::KEYPOINT_VISIBLE);
  };

  Status getDetailedStatus() const {
    return status_;
  };

 private:
  Status status_;
};

class Camera : public Sensor {
 public:
  ASLAM_POINTER_TYPEDEFS(Camera);
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  enum { CLASS_SERIALIZATION_VERSION = 1 };

  enum class Type {
    kPinhole = 0,
    kUnifiedProjection = 1,
    kLidar3D = 2,
  };


  // TODO(slynen) Enable commented out PropertyTree support
  // explicit Camera(const sm::PropertyTree& property_tree);
 protected:
  Camera() = delete;

  Camera(
      const Eigen::VectorXd& intrinsics,
      aslam::Distortion::UniquePtr& distortion, const uint32_t image_width,
      const uint32_t image_height, Type camera_type);

  Camera(
      const Eigen::VectorXd& intrinsics, const uint32_t image_width,
      const uint32_t image_height, Type camera_type);

  Sensor::Ptr cloneAsSensor() const override {
    return std::dynamic_pointer_cast<Sensor>(std::shared_ptr<Camera>(clone()));
  }

 public:
  virtual ~Camera() = default;

  std::ostream& operator<<(std::ostream& out) {
    this->printParameters(out, std::string(""));
    return out;
  };

  virtual aslam::Camera* clone() const = 0;

  uint8_t getSensorType() const override {
    return SensorType::kCamera;
  }

  std::string getSensorTypeString() const override {
    return static_cast<std::string>(kCameraIdentifier);
  }

 protected:
  Camera(const Camera& other)
      : Sensor(other),
        line_delay_nanoseconds_(other.line_delay_nanoseconds_),
        image_width_(other.image_width_),
        image_height_(other.image_height_),
        mask_(other.mask_.clone()),
        is_compressed_(other.is_compressed_),
        intrinsics_(other.intrinsics_),
        camera_type_(other.camera_type_),
        distortion_(nullptr) {
    CHECK(other.distortion_);
    distortion_.reset(other.distortion_->clone());
  };

  void operator=(const Camera&) = delete;

  bool isEqualCameraImpl(const Camera& other, const bool verbose = false) const;


 public:
  uint32_t imageWidth() const {
    return image_width_;
  }

  uint32_t imageHeight() const {
    return image_height_;
  }

  void setImageWidth(uint32_t image_width) {
    image_width_ = image_width;
  }

  void setImageHeight(uint32_t image_height) {
    image_height_ = image_height;
  }

  virtual void printParameters(
      std::ostream& out, const std::string& text) const;

  virtual int getParameterSize() const = 0;

  inline Type getType() const {
    return camera_type_;
  }



  const ProjectionResult project3(
      const Eigen::Ref<const Eigen::Vector3d>& point_3d,
      Eigen::Vector2d* out_keypoint) const;

  const ProjectionResult project3(
      const Eigen::Ref<const Eigen::Vector3d>& point_3d,
      Eigen::Vector2d* out_keypoint,
      Eigen::Matrix<double, 2, 3>* out_jacobian) const;

  virtual void project3Vectorized(
      const Eigen::Ref<const Eigen::Matrix3Xd>& points_3d,
      Eigen::Matrix2Xd* out_keypoints,
      std::vector<ProjectionResult>* out_results) const;

  virtual bool backProject3(
      const Eigen::Ref<const Eigen::Vector2d>& keypoint,
      Eigen::Vector3d* out_point_3d) const = 0;

  virtual void backProject3Vectorized(
      const Eigen::Ref<const Eigen::Matrix2Xd>& keypoints,
      Eigen::Matrix3Xd* out_points_3d,
      std::vector<unsigned char>* out_success) const;


  const ProjectionResult project4(
      const Eigen::Ref<const Eigen::Vector4d>& point_4d,
      Eigen::Vector2d* out_keypoint) const;

  const ProjectionResult project4(
      const Eigen::Ref<const Eigen::Vector4d>& point_4d,
      Eigen::Vector2d* out_keypoint,
      Eigen::Matrix<double, 2, 4>* out_jacobian) const;

  bool backProject4(
      const Eigen::Ref<const Eigen::Vector2d>& keypoint,
      Eigen::Vector4d* out_point_4d) const;



  const ProjectionResult project3Functional(
      const Eigen::Ref<const Eigen::Vector3d>& point_3d,
      const Eigen::VectorXd* intrinsics_external,
      const Eigen::VectorXd* distortion_coefficients_external,
      Eigen::Vector2d* out_keypoint) const;

  virtual const ProjectionResult project3Functional(
      const Eigen::Ref<const Eigen::Vector3d>& point_3d,
      const Eigen::VectorXd* intrinsics_external,
      const Eigen::VectorXd* distortion_coefficients_external,
      Eigen::Vector2d* out_keypoint,
      Eigen::Matrix<double, 2, 3>* out_jacobian_point,
      Eigen::Matrix<double, 2, Eigen::Dynamic>* out_jacobian_intrinsics,
      Eigen::Matrix<double, 2, Eigen::Dynamic>* out_jacobian_distortion)
      const = 0;


 public:

  uint64_t getLineDelayNanoSeconds() const {
    return line_delay_nanoseconds_;
  }

  void setLineDelayNanoSeconds(uint64_t line_delay_nano_seconds) {
    line_delay_nanoseconds_ = line_delay_nano_seconds;
  }

  virtual int64_t rollingShutterDelayNanoSeconds(
      const Eigen::Vector2d& keypoint) const {
    // Don't check validity. This allows points to wander in and out
    // of the frame during optimization
    return static_cast<int64_t>(keypoint(1)) * line_delay_nanoseconds_;
  }

  virtual int64_t maxRollingShutterDelayNanoSeconds() const {
    return this->imageHeight() * line_delay_nanoseconds_;
  }

  virtual uint32_t getNumberOfLines() const {
    if (line_delay_nanoseconds_ > 0) {
      return this->imageHeight();
    }
    return 1u;
  }

  enum class LineDelayMode : uint8_t { kColumns = 0, kRows = 1 };

  virtual LineDelayMode getLineDelayMode() const {
    return LineDelayMode::kRows;
  }


  bool hasCompressedImages() const {
    return is_compressed_;
  }

  void setCompressedImages(const bool is_compressed) {
    is_compressed_ = is_compressed;
  }



  bool isProjectable3(const Eigen::Ref<const Eigen::Vector3d>& point) const;

  bool isProjectable4(const Eigen::Ref<const Eigen::Vector4d>& point) const;

  template <typename DerivedKeyPoint>
  bool isKeypointVisible(
      const Eigen::MatrixBase<DerivedKeyPoint>& keypoint) const;

  template <typename DerivedKeyPoint>
  bool isKeypointVisibleWithMargin(
      const Eigen::MatrixBase<DerivedKeyPoint>& keypoint,
      typename DerivedKeyPoint::Scalar margin) const;



  virtual Eigen::Vector2d createRandomKeypoint() const = 0;

  virtual Eigen::Vector3d createRandomVisiblePoint(double depth) const = 0;



  aslam::Distortion* getDistortionMutable() {
    return CHECK_NOTNULL(distortion_.get());
  };

  const aslam::Distortion& getDistortion() const {
    return *CHECK_NOTNULL(distortion_.get());
  };

  void setDistortion(aslam::Distortion::UniquePtr& distortion) {
    distortion_ = std::move(distortion);
  };

  bool hasDistortion() const {
    CHECK(distortion_);
    return distortion_->getType() != aslam::Distortion::Type::kNoDistortion;
  }

  void removeDistortion() {
    distortion_.reset(new NullDistortion);
  };


  inline const Eigen::VectorXd& getParameters() const {
    return intrinsics_;
  };

  inline double* getParametersMutable() {
    return &intrinsics_.coeffRef(0, 0);
  };

  void setParameters(const Eigen::VectorXd& params) {
    CHECK_EQ(getParameterSize(), params.size());
    intrinsics_ = params;
  }

  virtual bool intrinsicsValid(const Eigen::VectorXd& intrinsics) const = 0;



  void setMask(const cv::Mat& mask);

  const cv::Mat& getMask() const;

  void clearMask();

  bool hasMask() const;

  inline bool isMasked(
      const Eigen::Ref<const Eigen::Vector2d>& keypoint) const {
    return keypoint[0] < 0.0 ||
           keypoint[0] >= static_cast<double>(image_width_) ||
           keypoint[1] < 0.0 ||
           keypoint[1] >= static_cast<double>(image_height_) ||
           (!mask_.empty() && mask_.at<uint8_t>(
                                  static_cast<int>(keypoint[1]),
                                  static_cast<int>(keypoint[0])) == 0);
  }



  template <typename DerivedCamera, typename DerivedDistortion>
  static typename DerivedCamera::Ptr construct(
      const Eigen::VectorXd& intrinsics, uint32_t imageWidth,
      uint32_t imageHeight, const Eigen::VectorXd& distortionParameters);


 private:
  bool isValidImpl() const = 0;
  void setRandomImpl() = 0;
  bool isEqualImpl(const Sensor& other, const bool verbose) const = 0;

  bool loadFromYamlNodeImpl(const YAML::Node&) override;
  void saveToYamlNodeImpl(YAML::Node*) const override;

 protected:
  uint64_t line_delay_nanoseconds_;
  uint32_t image_width_;
  uint32_t image_height_;
  cv::Mat_<uint8_t> mask_;
  bool is_compressed_;

  Eigen::VectorXd intrinsics_;

  Type camera_type_;

  aslam::Distortion::UniquePtr distortion_;
};
}  // namespace aslam
#include "camera-inl.h"
#endif  // ASLAM_CAMERAS_CAMERA_H_