Program Listing for File camera-pinhole.h

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

#ifndef ASLAM_CAMERAS_PINHOLE_CAMERA_H_
#define ASLAM_CAMERAS_PINHOLE_CAMERA_H_

#include <aslam/cameras/camera.h>
#include <aslam/cameras/distortion.h>
#include <aslam/common/crtp-clone.h>
#include <aslam/common/macros.h>
#include <aslam/common/types.h>

namespace aslam {

// Forward declarations.
class MappedUndistorter;
class NCamera;

class PinholeCamera : public aslam::Cloneable<Camera, PinholeCamera> {
  friend class NCamera;

  enum { kNumOfParams = 4 };

 public:
  ASLAM_POINTER_TYPEDEFS(PinholeCamera);

  enum { CLASS_SERIALIZATION_VERSION = 1 };
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  enum Parameters {
    kFu = 0,
    kFv = 1,
    kCu = 2,
    kCv = 3
  };

  // TODO(slynen) Enable commented out PropertyTree support
  // PinholeCamera(const sm::PropertyTree& config);


 public:
  PinholeCamera();

  PinholeCamera(const PinholeCamera& other) = default;
  void operator=(const PinholeCamera&) = delete;

 public:
  PinholeCamera(const Eigen::VectorXd& intrinsics, uint32_t image_width, uint32_t image_height,
                aslam::Distortion::UniquePtr& distortion);

  PinholeCamera(const Eigen::VectorXd& intrinsics, uint32_t image_width, uint32_t image_height);

  PinholeCamera(double focallength_cols, double focallength_rows,
                double imagecenter_cols, double imagecenter_rows,
                uint32_t image_width, uint32_t image_height,
                aslam::Distortion::UniquePtr& distortion);

  PinholeCamera(double focallength_cols, double focallength_rows,
                double imagecenter_cols, double imagecenter_rows,
                uint32_t image_width, uint32_t image_height);

  virtual ~PinholeCamera() {};

  friend std::ostream& operator<<(std::ostream& out, const PinholeCamera& camera);



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

  template <typename DerivedKeyPoint, typename DerivedPoint3d>
  inline const ProjectionResult evaluateProjectionResult(
      const Eigen::MatrixBase<DerivedKeyPoint>& keypoint,
      const Eigen::MatrixBase<DerivedPoint3d>& point_3d) const;



  // Get the overloaded non-virtual project3Functional(..) from base into scope.
  using Camera::project3Functional;

  template <typename ScalarType, typename DistortionType,
            typename MIntrinsics, typename MDistortion>
  const ProjectionResult project3Functional(
      const Eigen::Matrix<ScalarType, 3, 1>& point_3d,
      const Eigen::MatrixBase<MIntrinsics>& intrinsics_external,
      const Eigen::MatrixBase<MDistortion>& distortion_coefficients_external,
      Eigen::Matrix<ScalarType, 2, 1>* 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;



  virtual Eigen::Vector2d createRandomKeypoint() const;

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

  void getBorderRays(Eigen::MatrixXd& rays) const;


 public:

  Eigen::Matrix3d getCameraMatrix() const {
    Eigen::Matrix3d K;
    K << fu(), 0.0,  cu(),
         0.0,  fv(), cv(),
         0.0,  0.0,  1.0;
    return K;
  }

  double fu() const { return intrinsics_[Parameters::kFu]; };
  double fv() const { return intrinsics_[Parameters::kFv]; };
  double cu() const { return intrinsics_[Parameters::kCu]; };
  double cv() const { return intrinsics_[Parameters::kCv]; };

  inline static constexpr int parameterCount() {
      return kNumOfParams;
  }

  inline virtual int getParameterSize() const {
      return kNumOfParams;
  }

  static bool areParametersValid(const Eigen::VectorXd& parameters);

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

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


  template <typename DistortionType>
  static PinholeCamera::Ptr createTestCamera() {
    return PinholeCamera::Ptr(
        std::move(createTestCameraUnique<DistortionType>()));
  }

  template <typename DistortionType>
  static PinholeCamera::UniquePtr createTestCameraUnique() {
    Distortion::UniquePtr distortion = DistortionType::createTestDistortion();
    PinholeCamera::UniquePtr camera(
        new PinholeCamera(400, 300, 320, 240, 640, 480, distortion));
    CameraId id;
    generateId(&id);
    camera->setId(id);
    return std::move(camera);
  }

  static PinholeCamera::Ptr createTestCamera();

 private:
  static const double kMinimumDepth;

  bool isValidImpl() const override;
  void setRandomImpl() override;
  bool isEqualImpl(const Sensor& other, const bool verbose) const override;
};

}  // namespace aslam

#include "aslam/cameras/camera-pinhole-inl.h"

#endif  // ASLAM_CAMERAS_PINHOLE_CAMERA_H_