77 lines
4.2 KiB
C++
77 lines
4.2 KiB
C++
#ifndef H_Camera
|
|
#define H_Camera
|
|
|
|
#include <Eigen/Dense>
|
|
|
|
#define DEG2RAD 0.017453292519943295
|
|
|
|
/// @brief A camera class to perform frame transformations and projections.
|
|
class Camera {
|
|
public:
|
|
// Constructors
|
|
Camera();
|
|
|
|
// Accessors
|
|
Eigen::Vector3d const& GetForward() const;
|
|
Eigen::Vector3d const& GetUp() const;
|
|
Eigen::Matrix3d const& GetR() const;
|
|
Eigen::Matrix3d const& GetR_T() const;
|
|
double GetFOV() const;
|
|
|
|
// Setters following the Builder pattern. See https://en.wikipedia.org/wiki/Builder_pattern
|
|
Camera & SetForward(Eigen::Vector3d const& fwd_);
|
|
Camera & SetUp(Eigen::Vector3d const& up_);
|
|
Camera & SetR(Eigen::Matrix3d const& R_);
|
|
Camera & SetFOV(double fov);
|
|
|
|
// Methods
|
|
/// @brief Build the rotation matrix from the camera frame to the inertial frame from the fwd and up vectors.
|
|
Camera & BuildRotationMatrix();
|
|
|
|
/// @brief Orients the camera so that the forward vector points at the target. The position of the camera is (0,0,0).
|
|
Camera & SetTarget(Eigen::Vector3d const& target);
|
|
|
|
/// @brief Computes the ray direction in camera frame for a point (x,y) on the surface of the camera's sensor. p_norm is in normalized coordinates [-1;1].
|
|
/// @param p_norm Normalized coordinates of the point on the sensor.
|
|
/// @return The ray direction in camera frame, cartesian coordinates.
|
|
Eigen::Vector3d ComputeRayDirInCameraFrame(Eigen::Vector2d const& p_norm) const;
|
|
|
|
/// @brief Computes the ray direction in inertial frame for a point (x,y) on the surface of the camera's sensor. p_norm is in normalized coordinates [-1;1].
|
|
/// @param p_norm Normalized coordinates of the point on the sensor.
|
|
/// @return The ray direction in inertial frame, cartesian coordinates.
|
|
Eigen::Vector3d ComputeRayDirInInertialFrame(Eigen::Vector2d const& p_norm) const;
|
|
|
|
/// @brief Projects a point in camera frame (cartesian coordinates) to the sensor.
|
|
/// @param p_inertial Cartesian coordinates of the point to project, in camera frame.
|
|
/// @return Sensor coordinates of the projected point. The Z component is +/- tan(FOV/2). Negative Z means the point is in front of the camera.
|
|
Eigen::Vector3d ProjectPointToSensorCameraFrame(Eigen::Vector3d const& p_camera) const;
|
|
|
|
/// @brief Projects a point in inertial frame (cartesian coordinates) to the sensor.
|
|
/// @param p_inertial Cartesian coordinates of the point to project, in inertial frame.
|
|
/// @return Sensor coordinates of the projected point. The Z component is +/- tan(FOV/2). Negative Z means the point is in front of the camera.
|
|
Eigen::Vector3d ProjectPointToSensorInertialFrame(Eigen::Vector3d const& p_inertial) const;
|
|
|
|
/// @brief Checks if a point in camera frame is visible by the camera.
|
|
/// @param p_inertial Cartesian coordinates of the point to check, in camera frame.
|
|
/// @return True if the point is visible, false otherwise.
|
|
bool IsPointVisibleCameraFrame(Eigen::Vector3d const& p_camera) const;
|
|
|
|
/// @brief Checks if a point in inertial frame is visible by the camera.
|
|
/// @param p_inertial Cartesian coordinates of the point to check, in inertial frame.
|
|
/// @return True if the point is visible, false otherwise.
|
|
bool IsPointVisibleInertialFrame(Eigen::Vector3d const& p_inertial) const;
|
|
|
|
// Static methods
|
|
/// @brief Converts pixel coordinates to normalized coordinates. i is the row (y), j is the column (x). Normalized coordinates range from -1 to 1.
|
|
static Eigen::Vector2d PixelToNormalizedCoordinates(unsigned int i, unsigned int j, unsigned int width, unsigned int height);
|
|
|
|
private:
|
|
Eigen::Vector3d fwd; //!< Forward vector in inertial frame
|
|
Eigen::Vector3d up; //!< Up vector in inertial frame
|
|
Eigen::Matrix3d R; //!< Rotation matrix from camera frame to inertial frame
|
|
Eigen::Matrix3d R_T; //!< Rotation matrix from inertial frame to camera frame
|
|
double FOV; //!< Field of view in radians
|
|
double tanFOV2; //!< tan(FOV/2)
|
|
};
|
|
|
|
#endif
|