ecvCameraSensor.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        CloudViewer: www.cloudViewer.org                  -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.cloudViewer.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
 
#pragma once
 
// local
#include "LineSet.h"
#include "ecvOctree.h"
#include "ecvSensor.h"
 
// system
#include <unordered_set>
 
class ccPointCloud;
class ccMesh;
class ccImage;
class QDir;
 
class CV_DB_LIB_API ccCameraSensor : public ccSensor {
public:  // general
    struct CV_DB_LIB_API IntrinsicParameters {
        IntrinsicParameters();
 
        static void GetKinectDefaults(IntrinsicParameters& params);
 
        float vertFocal_pix; 
        float pixelSize_mm[2]; 
        float skew;            
        float vFOV_rad;        
        float zNear_mm;        
        float zFar_mm;         
        int arrayWidth;        
        int arrayHeight;       
        float principal_point[2]; 
 
        inline float horizFocal_pix() const {
            assert(pixelSize_mm[1] > 0);
            return (vertFocal_pix * pixelSize_mm[0]) / pixelSize_mm[1];
        }
    };
 
    enum DistortionModel {
        NO_DISTORTION_MODEL = 0, 
        SIMPLE_RADIAL_DISTORTION =
                1, 
        BROWN_DISTORTION =
                2, 
        EXTENDED_RADIAL_DISTORTION =
                3 
    };
 
    struct LensDistortionParameters {
        typedef QSharedPointer<LensDistortionParameters> Shared;
 
        virtual ~LensDistortionParameters() {}
 
        virtual DistortionModel getModel() const = 0;
    };
 
    struct CV_DB_LIB_API RadialDistortionParameters : LensDistortionParameters {
        typedef QSharedPointer<RadialDistortionParameters> Shared;
 
        RadialDistortionParameters() : k1(0), k2(0) {}
 
        // inherited from LensDistortionParameters
        inline virtual DistortionModel getModel() const override {
            return SIMPLE_RADIAL_DISTORTION;
        }
 
        float k1;
        float k2;
    };
 
    struct CV_DB_LIB_API ExtendedRadialDistortionParameters
        : RadialDistortionParameters {
        typedef QSharedPointer<RadialDistortionParameters> Shared;
 
        ExtendedRadialDistortionParameters()
            : RadialDistortionParameters(), k3(0) {}
 
        // inherited from LensDistortionParameters
        inline virtual DistortionModel getModel() const override {
            return EXTENDED_RADIAL_DISTORTION;
        }
 
        float k3;
    };
 
 
    struct CV_DB_LIB_API BrownDistortionParameters : LensDistortionParameters {
        typedef QSharedPointer<BrownDistortionParameters> Shared;
 
        BrownDistortionParameters();
 
        // inherited from LensDistortionParameters
        inline virtual DistortionModel getModel() const override {
            return BROWN_DISTORTION;
        }
 
        static void GetKinectDefaults(BrownDistortionParameters& params);
 
        float principalPointOffset[2];  
        float linearDisparityParams[2]; 
        float K_BrownParams[3]; 
        float P_BrownParams[2]; 
    };
 
 
    struct CV_DB_LIB_API FrustumInformation {
        FrustumInformation();
        ~FrustumInformation();
 
 
        bool initFrustumCorners();
 
        bool initFrustumHull();
 
        bool isComputed;
        bool drawFrustum;
        bool drawSidePlanes;
        ccPointCloud* frustumCorners;
        ccMesh* frustumHull;
        CCVector3 center; 
    };
 
    ccCameraSensor();
    ccCameraSensor(const ccCameraSensor& sensor);
    ccCameraSensor(const IntrinsicParameters& iParams);
 
    virtual ~ccCameraSensor() override;
 
    // inherited from ccHObject
    virtual CV_CLASS_ENUM getClassID() const override {
        return CV_TYPES::CAMERA_SENSOR;
    }
    virtual bool isSerializable() const override { return true; }
    virtual ccBBox getOwnBB(bool withGLFeatures = false) override;
    virtual ccBBox getOwnFitBB(ccGLMatrix& trans) override;
 
    // inherited from ccSensor
    virtual bool applyViewport() override;
 
public:  // getters and setters
 
    void setVertFocal_pix(float vertFocal_pix);
    inline float getVertFocal_pix() const {
        return m_intrinsicParams.vertFocal_pix;
    }
    inline float getHorizFocal_pix() const {
        return m_intrinsicParams.horizFocal_pix();
    }
 
    void setVerticalFov_rad(float fov_rad);
    inline float getVerticalFov_rad() const {
        return m_intrinsicParams.vFOV_rad;
    }
 
    const IntrinsicParameters& getIntrinsicParameters() const {
        return m_intrinsicParams;
    }
    void setIntrinsicParameters(const IntrinsicParameters& params);
 
    const LensDistortionParameters::Shared& getDistortionParameters() const {
        return m_distortionParams;
    }
    void setDistortionParameters(LensDistortionParameters::Shared params) {
        m_distortionParams = params;
    }
 
 
    bool getProjectionMatrix(ccGLMatrix& matrix);
 
public:  // frustum display
    inline bool frustumIsDrawn() const { return m_frustumInfos.drawFrustum; }
 
    inline void drawFrustum(bool state) { m_frustumInfos.drawFrustum = state; }
 
    inline bool frustumPlanesAreDrawn() const {
        return m_frustumInfos.drawSidePlanes;
    }
 
    inline void drawFrustumPlanes(bool state) {
        m_frustumInfos.drawSidePlanes = state;
    }
 
public:  // coordinate systems conversion methods
 
    bool fromLocalCoordToGlobalCoord(const CCVector3& localCoord,
                                     CCVector3& globalCoord) const;
 
 
    bool fromGlobalCoordToLocalCoord(const CCVector3& globalCoord,
                                     CCVector3& localCoord) const;
 
 
    bool fromLocalCoordToImageCoord(const CCVector3& localCoord,
                                    CCVector2& imageCoord,
                                    bool withLensError = true) const;
 
 
    bool fromImageCoordToLocalCoord(const CCVector2& imageCoord,
                                    CCVector3& localCoord,
                                    PointCoordinateType depth,
                                    bool withLensCorrection = true) const;
 
 
    bool fromGlobalCoordToImageCoord(const CCVector3& globalCoord,
                                     CCVector2& imageCoord,
                                     bool withLensError = true) const;
 
 
    bool fromImageCoordToGlobalCoord(const CCVector2& imageCoord,
                                     CCVector3& globalCoord,
                                     PointCoordinateType z0,
                                     bool withLensCorrection = true) const;
 
 
    bool fromRealImCoordToIdealImCoord(const CCVector2& real,
                                       CCVector2& ideal) const;
 
 
    // TODO
    // bool fromIdealImCoordToRealImCoord(const CCVector2& ideal, CCVector2&
    // real) const;
 
public:  // orthorectification tools
    struct KeyPoint {
        float x;
        float y;
        unsigned index;
 
        KeyPoint() : x(0), y(0), index(0) {}
 
        KeyPoint(float Px, float Py, unsigned indexInCloud)
            : x(Px), y(Py), index(indexInCloud) {}
    };
 
 
    ccPointCloud* orthoRectifyAsCloud(
            const ccImage* image,
            cloudViewer::GenericIndexedCloud* keypoints3D,
            std::vector<KeyPoint>& keypointsImage) const;
 
 
    ccImage* orthoRectifyAsImage(const ccImage* image,
                                 cloudViewer::GenericIndexedCloud* keypoints3D,
                                 std::vector<KeyPoint>& keypointsImage,
                                 double& pixelSize,
                                 double* minCorner = nullptr,
                                 double* maxCorner = nullptr,
                                 double* realCorners = nullptr) const;
 
 
    ccImage* orthoRectifyAsImageDirect(const ccImage* image,
                                       PointCoordinateType altitude,
                                       double& pixelSize,
                                       bool undistortImages = true,
                                       double* minCorner = nullptr,
                                       double* maxCorner = nullptr,
                                       double* realCorners = nullptr) const;
 
 
    static bool OrthoRectifyAsImages(
            std::vector<ccImage*> images,
            double a[],
            double b[],
            double c[],
            unsigned maxSize,
            QDir* outputDir = nullptr,
            std::vector<ccImage*>* orthoRectifiedImages = nullptr,
            std::vector<std::pair<double, double>>* relativePos = nullptr);
 
 
    bool computeOrthoRectificationParams(
            const ccImage* image,
            cloudViewer::GenericIndexedCloud* keypoints3D,
            std::vector<KeyPoint>& keypointsImage,
            double a[3],
            double b[3],
            double c[3]) const;
 
public:  // misc
 
    bool computeUncertainty(const CCVector2& pixel,
                            const float depth,
                            Vector3Tpl<ScalarType>& sigma) const;
 
 
    bool computeUncertainty(cloudViewer::ReferenceCloud* points,
                            std::vector<Vector3Tpl<ScalarType>>&
                                    accuracy /*, bool lensDistortion*/);
 
 
    QImage undistort(const QImage& image) const;
 
 
    ccImage* undistort(ccImage* image, bool inplace = true) const;
 
 
    bool isGlobalCoordInFrustum(
            const CCVector3& globalCoord /*, bool withLensCorrection*/) const;
 
 
    bool computeGlobalPlaneCoefficients(float planeCoefficients[6][4],
                                        CCVector3 ptsFrustum[8],
                                        CCVector3 edges[6],
                                        CCVector3& center);
 
    const cloudViewer::geometry::LineSet& getNearPlane() const {
        return m_nearPlane;
    }
    const cloudViewer::geometry::LineSet& getSideLines() const {
        return m_sideLines;
    }
    const cloudViewer::geometry::LineSet& getArrow() const { return m_arrow; }
    const cloudViewer::geometry::LineSet& getAxis() const { return m_axis; }
 
    void setPlaneColor(ecvColor::Rgb color) { m_plane_color = color; }
    const ecvColor::Rgb& getPlaneColor() const { return m_plane_color; }
 
    double getFocalLength() const { return m_focalLength; }
 
    virtual void clearDrawings() override;
    virtual void hideShowDrawings(CC_DRAW_CONTEXT& context) override;
 
public:  // helpers
    static float ConvertFocalPixToMM(float focal_pix, float ccdPixelSize_mm);
 
    static float ConvertFocalMMToPix(float focal_mm, float ccdPixelSize_mm);
 
    static float ComputeFovRadFromFocalPix(float focal_pix, int imageSize_pix);
 
    static float ComputeFovRadFromFocalMm(float focal_mm, float ccdSize_mm);
 
    void updateData();
 
protected:
    CCVector3 computeUpperLeftPoint() const;
 
    void computeProjectionMatrix();
 
 
    bool computeFrustumCorners();
 
    // Inherited from ccHObject
    bool toFile_MeOnly(QFile& out, short dataVersion) const override;
    short minimumFileVersion_MeOnly() const override;
    bool fromFile_MeOnly(QFile& in,
                         short dataVersion,
                         int flags,
                         LoadedIDMap& oldToNewIDMap) override;
    virtual void drawMeOnly(CC_DRAW_CONTEXT& context) override;
 
    ecvColor::Rgb m_plane_color;
 
    IntrinsicParameters m_intrinsicParams;
 
    LensDistortionParameters::Shared m_distortionParams;
 
 
    FrustumInformation m_frustumInfos;
 
    ccGLMatrix m_projectionMatrix;
    bool m_projectionMatrixIsValid;
 
    cloudViewer::geometry::LineSet m_nearPlane;
    cloudViewer::geometry::LineSet m_sideLines;
    cloudViewer::geometry::LineSet m_arrow;
    cloudViewer::geometry::LineSet m_axis;
    double m_focalLength;
};
 
class ccOctreeFrustumIntersector {
public:
 
    enum OctreeCellVisibility {
        CELL_OUTSIDE_FRUSTUM = 0,
        CELL_INSIDE_FRUSTUM = 1,
        CELL_INTERSECT_FRUSTUM = 2,
    };
 
    ccOctreeFrustumIntersector() : m_associatedOctree(nullptr) {}
 
    bool build(cloudViewer::DgmOctree* octree);
 
    OctreeCellVisibility positionFromFrustum(
            cloudViewer::DgmOctree::CellCode truncatedCode,
            unsigned char level) const {
        assert(m_associatedOctree);
 
        std::unordered_set<cloudViewer::DgmOctree::CellCode>::const_iterator
                got = m_cellsInFrustum[level].find(truncatedCode);
        if (got != m_cellsInFrustum[level].end()) return CELL_INSIDE_FRUSTUM;
        got = m_cellsIntersectFrustum[level].find(truncatedCode);
        if (got != m_cellsIntersectFrustum[level].end())
            return CELL_INTERSECT_FRUSTUM;
        return CELL_OUTSIDE_FRUSTUM;
    }
 
 
    void computeFrustumIntersectionWithOctree(
            std::vector<std::pair<unsigned, CCVector3>>& pointsToTest,
            std::vector<unsigned>& inCameraFrustum,
            const float planesCoefficients[6][4],
            const CCVector3 ptsFrustum[8],
            const CCVector3 edges[6],
            const CCVector3& center);
 
 
    void computeFrustumIntersectionByLevel(
            unsigned char level,
            cloudViewer::DgmOctree::CellCode parentTruncatedCode,
            OctreeCellVisibility parentResult,
            const float planesCoefficients[6][4],
            const CCVector3 ptsFrustum[8],
            const CCVector3 edges[6],
            const CCVector3& center);
 
 
    OctreeCellVisibility separatingAxisTest(
            const CCVector3& bbMin,
            const CCVector3& bbMax,
            const float planesCoefficients[6][4],
            const CCVector3 frustumCorners[8],
            const CCVector3 frustumEdges[6],
            const CCVector3& frustumCenter);
 
protected:
    cloudViewer::DgmOctree* m_associatedOctree;
 
    // contains the truncated code of the cells built in the octree
    std::unordered_set<cloudViewer::DgmOctree::CellCode>
            m_cellsBuilt[cloudViewer::DgmOctree::MAX_OCTREE_LEVEL + 1];
    // contains the truncated code of the cells INSIDE the frustum
    std::unordered_set<cloudViewer::DgmOctree::CellCode>
            m_cellsInFrustum[cloudViewer::DgmOctree::MAX_OCTREE_LEVEL + 1];
    // contains the truncated code of the cells INTERSECTING the frustum
    std::unordered_set<cloudViewer::DgmOctree::CellCode>
            m_cellsIntersectFrustum[cloudViewer::DgmOctree::MAX_OCTREE_LEVEL +
                                    1];
};