RealSensePointSmoother.h

#pragma once
#include "RasterPointScanner/Headers/ViewportPointSmoother.hpp"
#include "RasterPointScanner/Headers/RasterFrame.h"
#include <librealsense2/rs.hpp>
namespace NDEVR
{
    class RealSensePointSmoother : public ViewportPointSmoother<ColorNormalNode>
    {
    public:
        RealSensePointSmoother(rs2_intrinsics intrinsics)
        {
            m_intrinsics.setScreenSize(Vector<2, uint04>(intrinsics.width, intrinsics.height));
            m_intrinsics.setFx({ intrinsics.fx,intrinsics.fy });
            m_intrinsics.pp[X] = intrinsics.ppx;
            m_intrinsics.pp[Y] = intrinsics.ppy;
            for(uint01 i = 0; i < 5; i++)
                m_intrinsics.coeffs[i] = intrinsics.coeffs[i];
        }
        /*Vertex<3, fltp04> pixelToPoint(Vertex<3, fltp04> pixel) const final override
        {
            /*
            pixel[X] *= m_inv_pixel_multiplier;
            pixel[Y] *= m_inv_pixel_multiplier;
            Vector<3, fltp04> location;
            rs2_deproject_pixel_to_point(&location[0], &m_intrinics, &pixel[0], pixel[2]);
            return location;
            pixel[X] *= m_inv_pixel_multiplier;
            pixel[Y] *= m_inv_pixel_multiplier;
 
            const auto& intr = m_intrinics;
            const fltp04 ppx = intr.ppx;
            const fltp04 ppy = intr.ppy;
            const fltp04* k = intr.coeffs; // distortion coeffs: [k1, k2, p1, p2, k3]
 
            fltp04 x = (pixel[X] - ppx) * m_inv_f[X];
            fltp04 y = (pixel[Y] - ppy) * m_inv_f[Y];
            const fltp04 xo = x;
            const fltp04 yo = y;
 
            for (int i = 0; i < 5; ++i)
            {
                const fltp04 r2 = x * x + y * y;
                const fltp04 r4 = r2 * r2;
                const fltp04 r6 = r4 * r2;
 
                // inverse distortion scale
                const fltp04 icdist = fltp04(1) / (fltp04(1) + k[0] * r2 + k[1] * r4 + k[4] * r6);
 
                const fltp04 xq = x * icdist;
                const fltp04 yq = y * icdist;
 
                const fltp04 xq2 = xq * xq;
                const fltp04 yq2 = yq * yq;
 
                const fltp04 delta_x = 2 * k[2] * xq * yq + k[3] * (r2 + 2 * xq2);
                const fltp04 delta_y = 2 * k[3] * xq * yq + k[2] * (r2 + 2 * yq2);
 
                x = (xo - delta_x) * icdist;
                y = (yo - delta_y) * icdist;
            }
 
            return {
                pixel[Z] * x,
                pixel[Z] * y,
                pixel[Z]
            };
        }
        Vertex<2, fltp04> pointToPixel(Vertex<3, fltp04> location) const final override
        {
            /*
            if (!m_camera_location_bounds.contains(location.as<2, fltp04>() / cast<fltp04>(location[Z])))
                return Constant<Vector<2, fltp04>>::Invalid;
                Vertex<2, fltp04> pixel;
                rs2_project_point_to_pixel(&pixel[0], &m_intrinics, &location[0]);
                return m_pixel_multiplier * pixel;
            }
            Vertex<2, fltp04> xy;
            {
                const fltp04 iz = fltp04(1) / cast<fltp04>(location[Z]);
                const fltp04 x0 = cast<fltp04>(location[X]) * iz;
                const fltp04 y0 = cast<fltp04>(location[Y]) * iz;
                xy[X] = x0;
                xy[Y] = y0;
            }
            if (!m_camera_location_bounds.contains(xy))
                return Constant<Vector<2, fltp04>>::Invalid;
 
            const fltp04 x0 = xy[X];
            const fltp04 y0 = xy[Y];
            const fltp04 x2 = x0 * x0;
            const fltp04 y2 = y0 * y0;
            const fltp04 r2 = x2 + y2;
 
            const fltp04 k1 = m_intrinics.coeffs[0];
            const fltp04 k2 = m_intrinics.coeffs[1];
            const fltp04 p1 = m_intrinics.coeffs[2];
            const fltp04 p2 = m_intrinics.coeffs[3];
            const fltp04 k3 = m_intrinics.coeffs[4];
 
            const fltp04 f = std::fma(std::fma(std::fma(k3, r2, k2), r2, k1), r2, fltp04(1));
            const fltp04 xr = x0 * f;
            const fltp04 yr = y0 * f;
 
            const fltp04 two_xy = fltp04(2) * x0 * y0;
            const fltp04 dx = xr + p1 * two_xy + p2 * (r2 + fltp04(2) * x2);
            const fltp04 dy = yr + p2 * two_xy + p1 * (r2 + fltp04(2) * y2);
 
            Vertex<2, fltp04> pixel;
            pixel[0] = std::fma(dx, m_intrinics.fx, m_intrinics.ppx);
            pixel[1] = std::fma(dy, m_intrinics.fy, m_intrinics.ppy);
            return m_pixel_multiplier * pixel;
        }*/
        /*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_intrinics.width);
            m_inv_pixel_multiplier = 1.0f / m_pixel_multiplier;
        }
        RealSensePointSmoother* clone()
        {
            RealSensePointSmoother* smoother = new RealSensePointSmoother(m_intrinics);
            smoother->setViewSize(m_view_size_i);
            smoother->setBlockTransform(m_block_mat);
            smoother->m_frame_reduction = m_frame_reduction;
            return smoother;
        }*/
    };
}