DepthAIPointSmoother.h

#pragma once
#include "RasterPointScanner/Headers/ViewportPointSmoother.hpp"
#include "RasterPointScanner/Headers/RasterFrame.h"
#include "Base/Headers/Matrix.hpp"
namespace NDEVR
{
    class DepthAISmoother : public ViewportPointSmoother<ColorNormalNode>
    {
    public:
        DepthAISmoother(const Vector<2, uint04>& size, const Matrix<fltp04>& intrinsics)
            : m_intrinics(intrinsics)
            , m_original_size(size)
        {
            m_view_size_i = size;
            m_view_size_f = m_view_size_i.as<2, fltp04>();
            m_inv_fov[X] = 1.0 / m_intrinics[0][0];
            m_inv_fov[Y] = 1.0 / m_intrinics[1][1];
        }

        Vertex<3, fltp04> pixelToPoint(Vertex<3, fltp04> pixel) const final override
        {
            pixel *= m_inv_pixel_multiplier;
            fltp04 cx = m_intrinics[0][2];
            fltp04 cy = m_intrinics[1][2];
            pixel[X] = (pixel[X] - cx) * pixel[Z] * m_inv_fov[X];
            pixel[Y] = (pixel[Y] - cy) * pixel[Z] * m_inv_fov[Y];
            return pixel;
        }

        Vertex<2, fltp04> pointToPixel(Vertex<3, fltp04> location) const final override
        {
            fltp04 fx = m_intrinics[0][0];
            fltp04 fy = m_intrinics[1][1];
            fltp04 cx = m_intrinics[0][2];
            fltp04 cy = m_intrinics[1][2];
            location[X] = (location[X] / location[Z] * fx) + cx;
            location[Y] = (location[Y] / location[Z] * fy) + cy;
            return m_pixel_multiplier * location.as<2, fltp04>();
        }

        void setViewSize(const Vector<2, uint04>& view_size) final override
        {
            ViewportPointSmoother::setViewSize(view_size);
            m_pixel_multiplier = m_view_size_f[X] / cast<fltp04>(m_original_size[X]);
            m_inv_pixel_multiplier = 1.0 / m_pixel_multiplier;
        }

        DepthAISmoother* clone()
        {
            DepthAISmoother* smoother = new DepthAISmoother(m_original_size, m_intrinics);
            smoother->setViewSize(m_view_size_i);
            smoother->setBlockTransform(m_block_mat);
            smoother->m_frame_reduction = m_frame_reduction;
            return smoother;
        }
    protected:
        Matrix<fltp04> m_intrinics;                  
        Vector<2, fltp04> m_inv_fov;                 
        Vector<2, uint04> m_original_size;            
        fltp04 m_pixel_multiplier = 1.0f;            
        fltp04 m_inv_pixel_multiplier = 1.0f;        
    };
}