360toPerspective/cpp/src/Camera.cpp

#include <Camera.hpp>

Camera::Camera()
 : fwd(Eigen::Vector3d::UnitX()), up(Eigen::Vector3d::UnitZ()), R(Eigen::Matrix3d::Identity()), R_T(Eigen::Matrix3d::Identity()), FOV(90.0*DEG2RAD), tanFOV2(tan(FOV/2.0))
{
    BuildRotationMatrix();
}

// Getters
const Eigen::Vector3d& Camera::GetForward() const {
    return fwd;
}

const Eigen::Vector3d& Camera::GetUp() const {
    return up;
}

const Eigen::Matrix3d& Camera::GetR() const {
    return R;
}

const Eigen::Matrix3d& Camera::GetR_T() const {
    return R_T;
}

double Camera::GetFOV() const {
    return FOV;
}

// Setters
Camera& Camera::SetForward(const Eigen::Vector3d& fwd_) {
    fwd = fwd_;
    return *this;
}

Camera& Camera::SetUp(const Eigen::Vector3d& up_) {
    up = up_;
    return *this;
}

Camera& Camera::SetR(const Eigen::Matrix3d& R_) {
    R = R_;
    R_T = R.transpose();
    return *this;
}

Camera& Camera::SetFOV(double fov) {
    FOV = fov;
    tanFOV2 = tan(FOV/2.0);
    return *this;
}

Camera & Camera::BuildRotationMatrix() {
    Eigen::Vector3d left   = up.cross(fwd);
    left                   = left/left.norm();
    Eigen::Vector3d cam_up = fwd.cross(left);
    R.col(0)               = -left;        // x-axis is to the right
    R.col(1)               = cam_up;       // y-axis is up
    R.col(2)               = -fwd;         // z-axis is backwards
    R_T = R.transpose();
    return *this;
}

Camera & Camera::SetTarget(Eigen::Vector3d const& target) {
    fwd = target/target.norm();
    BuildRotationMatrix();
    return *this;
}

Eigen::Vector3d Camera::ComputeRayDirInCameraFrame(Eigen::Vector2d const& p_norm) const {
    return Eigen::Vector3d(p_norm(0)*tanFOV2, p_norm(1)*tanFOV2, -1.0).normalized();
}

Eigen::Vector3d Camera::ComputeRayDirInInertialFrame(Eigen::Vector2d const& p_norm) const {
    return R*ComputeRayDirInCameraFrame(p_norm);
}

Eigen::Vector3d Camera::ProjectPointToSensorCameraFrame(Eigen::Vector3d const& p_camera) const {
    Eigen::Vector3d p_sensor = p_camera;
    if(p_camera[2] != 0.0)
        p_sensor /= tanFOV2*std::abs(p_camera[2]);
    return p_sensor;
}

Eigen::Vector3d Camera::ProjectPointToSensorInertialFrame(Eigen::Vector3d const& p_inertial) const {
    return ProjectPointToSensorCameraFrame(R_T*p_inertial);
}

bool Camera::IsPointVisibleCameraFrame(Eigen::Vector3d const& p_camera) const {
    Eigen::Vector3d p_sensor = ProjectPointToSensorCameraFrame(p_camera);
    return (p_sensor[0] >= -1.0) && (p_sensor[0] <= 1.0) && (p_sensor[1] >= -1.0) && (p_sensor[1] <= 1.0) && (p_sensor[2] < 0.0);
}

bool Camera::IsPointVisibleInertialFrame(Eigen::Vector3d const& p_inertial) const {
    Eigen::Vector3d p_sensor = ProjectPointToSensorInertialFrame(p_inertial);
    return (p_sensor[0] >= -1.0) && (p_sensor[0] <= 1.0) && (p_sensor[1] >= -1.0) && (p_sensor[1] <= 1.0) && (p_sensor[2] < 0.0);
}

Eigen::Vector2d Camera::PixelToNormalizedCoordinates(unsigned int i, unsigned int j, unsigned int width, unsigned int height) {
    return Eigen::Vector2d(2.0*((double)j)/(width - 1) - 1.0, -2.0*((double)i)/(height - 1) + 1.0);
}