IntrinsicBase.hpp

// This file is part of the AliceVision project.
// Copyright (c) 2016 AliceVision contributors.
// Copyright (c) 2012 openMVG contributors.
// This Source Code Form is subject to the terms of the Mozilla Public License,
// v. 2.0. If a copy of the MPL was not distributed with this file,
// You can obtain one at https://mozilla.org/MPL/2.0/.
 
#pragma once
 
#include <aliceVision/types.hpp>
#include <aliceVision/numeric/numeric.hpp>
#include <aliceVision/camera/cameraCommon.hpp>
#include <aliceVision/camera/IntrinsicInitMode.hpp>
#include <aliceVision/geometry/Pose3.hpp>
#include <aliceVision/version.hpp>
 
#include <vector>
 
namespace aliceVision {
namespace camera {
 
class IntrinsicBase
{
  public:
    using sptr = std::shared_ptr<IntrinsicBase>;
    using ptr = IntrinsicBase*;
 
  public:
    explicit IntrinsicBase(unsigned int width, unsigned int height, const std::string& serialNumber = "")
      : _w(width),
        _h(height),
        _serialNumber(serialNumber)
    {}
 
    virtual ~IntrinsicBase() = default;
 
    inline bool isLocked() const { return _locked; }
 
    inline unsigned int w() const { return _w; }
 
    inline unsigned int h() const { return _h; }
 
    inline double sensorWidth() const { return _sensorWidth; }
 
    inline double sensorHeight() const { return _sensorHeight; }
 
    inline const std::string& serialNumber() const { return _serialNumber; }
 
    inline EInitMode getInitializationMode() const { return _initializationMode; }
 
    virtual bool operator==(const IntrinsicBase& other) const;
 
    inline bool operator!=(const IntrinsicBase& other) const { return !(*this == other); }
 
    Vec2 transformProject(const geometry::Pose3& pose, const Vec4& pt3D, bool applyDistortion = true) const
    {
        return transformProject(pose.getHomogeneous(), pt3D, applyDistortion);
    }
 
    virtual Vec2 transformProject(const Eigen::Matrix4d& pose, const Vec4& pt3D, bool applyDistortion = true) const = 0;
 
    virtual Vec2 project(const Vec4& pt3D, bool applyDistortion = true) const = 0;
 
    Vec3 backprojectTransform(const Vec2& pt2D,
                              bool applyUndistortion = true,
                              const geometry::Pose3& pose = geometry::Pose3(),
                              double depth = 1.0) const;
 
    Vec3 backProjectUnit(const Vec2& pt2D) const;
 
    Vec4 getCartesianfromSphericalCoordinates(const Vec3& pt);
 
    Eigen::Matrix<double, 4, 3> getDerivativeCartesianfromSphericalCoordinates(const Vec3& pt);
 
    virtual Eigen::Matrix<double, 2, 3> getDerivativeTransformProjectWrtPoint3(const Eigen::Matrix4d& pose, const Vec4& pt3D) const = 0;
 
    virtual Eigen::Matrix<double, 2, Eigen::Dynamic> getDerivativeTransformProjectWrtParams(const Eigen::Matrix4d& pos, const Vec4& pt3D) const = 0;
 
    virtual Eigen::Matrix<double, 3, Eigen::Dynamic> getDerivativeBackProjectUnitWrtParams(const Vec2& pt2D) const = 0;
 
    inline Vec2 residual(const geometry::Pose3& pose, const Vec4& X, const Vec2& x, bool applyDistortion = true) const
    {
        // We will compare to an undistorted point, so always ignore the distortion when computing coordinates
        const Vec2 proj = this->transformProject(pose, X, false);
 
        return ((applyDistortion) ? this->getUndistortedPixel(x) : x) - proj;
    }
 
    inline Mat2X residuals(const geometry::Pose3& pose, const Mat3X& X, const Mat2X& x) const
    {
        assert(X.cols() == x.cols());
        const std::size_t numPts = x.cols();
        Mat2X residuals = Mat2X::Zero(2, numPts);
        for (std::size_t i = 0; i < numPts; ++i)
        {
            residuals.col(i) = residual(pose, ((const Vec3&)X.col(i)).homogeneous(), x.col(i));
        }
        return residuals;
    }
 
    inline void lock() { _locked = true; }
 
    inline void unlock() { _locked = false; }
 
    inline void setWidth(unsigned int width) { _w = width; }
 
    inline void setHeight(unsigned int height) { _h = height; }
 
    inline void setSensorWidth(double width) { _sensorWidth = width; }
 
    inline void setSensorHeight(double height) { _sensorHeight = height; }
 
    inline void setSerialNumber(const std::string& serialNumber) { _serialNumber = serialNumber; }
 
    inline void setInitializationMode(EInitMode initializationMode) { _initializationMode = initializationMode; }
 
    // Virtual members
 
    virtual IntrinsicBase* clone() const = 0;
 
    virtual void assign(const IntrinsicBase& other) = 0;
 
    virtual EINTRINSIC getType() const = 0;
 
    std::string getTypeStr() const { return EINTRINSIC_enumToString(getType()); }
 
    virtual std::vector<double> getParameters() const = 0;
 
    virtual std::size_t getParametersSize() const = 0;
 
    virtual bool updateFromParams(const std::vector<double>& params) = 0;
 
    virtual bool importFromParams(const std::vector<double>& params, const Version& inputVersion) = 0;
 
    virtual Vec2 cam2ima(const Vec2& p) const = 0;
 
    virtual Vec2 ima2cam(const Vec2& p) const = 0;
 
    virtual bool hasDistortion() const { return false; }
 
    virtual Vec2 addDistortion(const Vec2& p) const = 0;
 
    virtual Vec2 removeDistortion(const Vec2& p) const = 0;
 
    virtual Vec2 getUndistortedPixel(const Vec2& p) const = 0;
 
    virtual Vec2 getDistortedPixel(const Vec2& p) const = 0;
 
    virtual void setDistortionInitializationMode(EInitMode distortionInitializationMode) = 0;
 
    virtual EInitMode getDistortionInitializationMode() const = 0;
 
    virtual double imagePlaneToCameraPlaneError(double value) const = 0;
 
    virtual double pixelProbability() const = 0;
 
    virtual bool isValid() const { return _w != 0 && _h != 0; }
 
    virtual bool isVisibleRay(const Vec3& ray) const = 0;
 
    virtual bool isVisible(const Vec2& pix) const;
 
    virtual bool isVisible(const Vec2f& pix) const;
 
    virtual float getMaximalDistortion(double minRadius, double maxRadius) const;
 
    virtual std::size_t hashValue() const;
 
    virtual void rescale(float factorW, float factorH);
 
    virtual Vec3 toUnitSphere(const Vec2& pt) const = 0;
 
    virtual double getHorizontalFov() const = 0;
 
    virtual double getVerticalFov() const = 0;
 
    virtual void initializeState()
    {
        if (_locked)
        {
            _state = EEstimatorParameterState::CONSTANT;
        }
        else
        {
            _state = EEstimatorParameterState::REFINED;
        }
    }
 
    EEstimatorParameterState getState() const { return _state; }
 
    void setState(EEstimatorParameterState state) { _state = state; }
 
  protected:
    std::string _serialNumber;
    double _sensorWidth = 36.0;
    double _sensorHeight = 24.0;
    unsigned int _w = 0;
    unsigned int _h = 0;
 
    EInitMode _initializationMode = EInitMode::NONE;
 
    bool _locked = false;
    EEstimatorParameterState _state = EEstimatorParameterState::REFINED;
};
 
inline Vec3 applyIntrinsicExtrinsic(const geometry::Pose3& pose, const IntrinsicBase* intrinsic, const Vec2& x)
{
    // x = (u, v, 1.0)  // image coordinates
    // X = R.t() * K.inv() * x + C // Camera world point
    // getting the ray:
    // ray = X - C = R.t() * K.inv() * x
    return (pose.rotation().transpose() * intrinsic->toUnitSphere(intrinsic->removeDistortion(intrinsic->ima2cam(x)))).normalized();
}
 
inline double angleBetweenRays(const Vec3& ray1, const Vec3& ray2)
{
    const double mag = ray1.norm() * ray2.norm();
    const double dotAngle = ray1.dot(ray2);
    return radianToDegree(acos(clamp(dotAngle / mag, -1.0 + 1.e-8, 1.0 - 1.e-8)));
}
 
inline double angleBetweenRays(const geometry::Pose3& pose1,
                               const IntrinsicBase* intrinsic1,
                               const geometry::Pose3& pose2,
                               const IntrinsicBase* intrinsic2,
                               const Vec2& x1,
                               const Vec2& x2)
{
    const Vec3 ray1 = applyIntrinsicExtrinsic(pose1, intrinsic1, x1);
    const Vec3 ray2 = applyIntrinsicExtrinsic(pose2, intrinsic2, x2);
    return angleBetweenRays(ray1, ray2);
}
 
inline double angleBetweenRays(const geometry::Pose3& pose1, const geometry::Pose3& pose2, const Vec3& pt3D)
{
    const Vec3 ray1 = pt3D - pose1.center();
    const Vec3 ray2 = pt3D - pose2.center();
    return angleBetweenRays(ray1, ray2);
}
 
}  // namespace camera
 
}  // namespace aliceVision