Program Listing for File camera-unified-projection.h

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

#ifndef ASLAM_UNIFIED_PROJECTION_CAMERA_H_
#define ASLAM_UNIFIED_PROJECTION_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 UnifiedProjectionCamera
    : public aslam::Cloneable<Camera, UnifiedProjectionCamera> {
  friend class NCamera;

  enum { kNumOfParams = 5 };

 public:
  ASLAM_POINTER_TYPEDEFS(UnifiedProjectionCamera);

  enum { CLASS_SERIALIZATION_VERSION = 1 };
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

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

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


 public:
  UnifiedProjectionCamera();

  UnifiedProjectionCamera(const UnifiedProjectionCamera& other) = default;

  void operator=(const UnifiedProjectionCamera&) = delete;

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

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

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

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

  virtual ~UnifiedProjectionCamera() {};

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



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

  const ProjectionResult evaluateProjectionResult(const Eigen::Ref<const Eigen::Vector2d>& keypoint,
                                                  const Eigen::Vector3d& point_3d) const;


  bool isUndistortedKeypointValid(const double& rho2_d, const double& xi) const;

  bool isLiftable(const Eigen::Ref<const Eigen::Vector2d>& keypoint) const;



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

  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;


 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 xi() const { return intrinsics_[Parameters::kXi]; };
  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]; };
  double fov_parameter(double xi) const { return (xi <= 1.0) ? xi : (1 / xi); };

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

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

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

  virtual int getParameterSize() const {
    return kNumOfParams;
  }

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


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

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

  static UnifiedProjectionCamera::Ptr createTestCamera();

 private:
  static constexpr double kMinimumDepth = 1e-10;

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

}  // namespace aslam

#endif  // ASLAM_UNIFIED_PROJECTION_CAMERA_H_