sources/libs/nvmultiobjecttracker/include/modules/utility/cv_functions.hpp

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: Copyright (c) 2021-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 
#ifndef CV_FUNCTIONS_HPP
#define CV_FUNCTIONS_HPP
 
#include <sys/stat.h>
#include <stdexcept>
#include <iostream>
#include <vector>
 
#if defined(VIZ_DEBUG) || defined(VIZ_DEBUG_SEG)
#include <opencv2/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#endif
 
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
 
#include "modules/utility/dataTypes.hpp"
 
namespace CV_FUNCS {
 
Eigen::VectorXd convertBboxToZ(const NvMotDataTypes::Rect2f &bbox, const bool useAspc=false);
 
void convexHull(const std::vector<Eigen::Vector2d> &points, std::vector<Eigen::Vector2d> &hull);
 
void convexHull(const std::vector<Eigen::Vector2f> &points, std::vector<Eigen::Vector2f> &hull);
 
NvMotDataTypes::Rect2f boundingRect(const std::vector<Eigen::Vector2d> &points);
 
NvMotDataTypes::Rect2f boundingRect(float *mask, const int width, const int height);
 
#ifdef VIZ_DEBUG
 
inline Eigen::Vector2f convertOcvPtToEigenPt(const cv::Point2d pt_ocv)
{
    Eigen::Vector2f pt_e;
    pt_e << pt_ocv.x, pt_ocv.y;
    return pt_e;
}
#endif
 
inline NvMotDataTypes::Rect2f constructRect2f(
    const float minX=0, const float minY=0, const float width=0, const float height=0
)
{
    Eigen::Vector2f min(minX, minY);
    Eigen::Vector2f max(minX + width, minY + height);
 
    return NvMotDataTypes::Rect2f(min, max);
}
 
inline NvMotDataTypes::Rect2f displaceBbox(
    const NvMotDataTypes::Rect2f &bbox, const float displacement_x, const float displacement_y
)
{
    Eigen::Vector2f min = bbox.min();
    Eigen::Vector2f max = bbox.max();
    min(0) += displacement_x;
    max(0) += displacement_x;
    min(1) += displacement_y;
    max(1) += displacement_y;
 
    return NvMotDataTypes::Rect2f(min, max);
}
 
inline Eigen::Vector2f getBboxCenterPt(const NvMotDataTypes::Rect2f &bbox)
{
    const Eigen::Vector2f min = bbox.min();
    const Eigen::Vector2f size = bbox.sizes();
 
    return Eigen::Vector2f(min(0) + size(0)/2, min(1) + size(1)/2);
}
 
inline const std::string getEigenRectString(const NvMotDataTypes::Rect2f &bbox)
{
    std::stringstream out;
    out << "[" << bbox.sizes()(0) << " x " << bbox.sizes()(1) << " from (" << bbox.min()(0) << ", " << bbox.min()(1) << ")]";
    return out.str();
}
 
inline bool isBboxEqual(const NvMotDataTypes::Rect2f &bbox1, const NvMotDataTypes::Rect2f &bbox2)
{
    return bbox1.min() == bbox2.min() && bbox1.max() == bbox2.max();
}
 
inline int modul(int a, int b)
{
    return ((a % b) + b) % b;
}
 
inline NvMotDataTypes::Rect2f getBoxWithAspectRatio(const NvMotDataTypes::Rect2f &bbox, const float aspectRatio)
{
    Eigen::Vector2f min_new = bbox.min();
    Eigen::Vector2f max_new = bbox.max();
 
    const float width_prev = max_new(0) - min_new(0);
    const float height = max_new(1) - min_new(1);
    const float width_new = height * aspectRatio;
    min_new(0) -= (width_new - width_prev) / 2.0;
    max_new(0) += (width_new - width_prev) / 2.0;
 
    return NvMotDataTypes::Rect2f(min_new, max_new);
}
 
#if defined(VIZ_DEBUG) || defined(VIZ_DEBUG_SEG)
 
inline NvMotDataTypes::Rect2f convertCvRect2f2MotRect2f(const cv::Rect2f &bbox)
{
    Eigen::Vector2f min; min << bbox.x, bbox.y;
    Eigen::Vector2f max; max << bbox.x + bbox.width, bbox.y + bbox.height;
 
    return NvMotDataTypes::Rect2f(min, max);
}
 
inline cv::Rect2f convertMotRect2f2CvRect2f(const NvMotDataTypes::Rect2f &bbox)
{
    Eigen::Vector2f min = bbox.min();
 
    return cv::Rect2f(min(0), min(1), bbox.sizes()(0), bbox.sizes()(1));
}
#endif
 
inline NvMotDataTypes::Rect2f convertXToBbox(const Eigen::VectorXd &x, const bool useAspc=false)
{
    assert(x.rows() >= 4 || x.cols() >= 4);
 
    double h = x(3);
 
    double w = useAspc ? x(2) * h : x(2);
 
    return constructRect2f(x(0)-w/2, x(1)-h/2, w, h);
}
 
inline NvMotDataTypes::RowMajorMatrixXf getHanningWindow(const int width, const int height, float offsetFactor_y = 0.f)
{
    Eigen::RowVectorXf hann_rows(height);
    Eigen::RowVectorXf hann_cols(width);
    int NN = height - 1;
 
    int offset_y =  NN * offsetFactor_y;
 
    if(NN != 0)
    {
        for (int i = 0; i < height; ++i)
        {
            int i_p = i-offset_y > height ? 0 : i-offset_y;
            hann_rows(i) = (float)(1.0/2.0 * (1.0 - cos(2*M_PI*(i_p)/NN)));
        }
    }
    NN = width - 1;
    if(NN != 0)
    {
        for (int i = 0; i < width; ++i)
        {
            hann_cols(i) = (float)(1.0/2.0 * (1.0 - cos(2*M_PI*i/NN)));
        }
    }
 
    NvMotDataTypes::RowMajorMatrixXf hann_window = hann_rows.transpose() * hann_cols;
 
    return hann_window;
}
 
inline float computeSizeScore(const NvMotDataTypes::Rect2f &currBox, const NvMotDataTypes::Rect2f &prevBox)
{
    const float areaPrev = prevBox.volume();
    const float areaCurr = currBox.volume();
 
    if(prevBox.isEmpty() || areaPrev < std::numeric_limits<float>::epsilon()
            || currBox.isEmpty() || areaCurr < std::numeric_limits<float>::epsilon())
        return 0;
 
    float sizeRatio = areaCurr / areaPrev;
 
    if(sizeRatio > 1)
        sizeRatio = 1.f / sizeRatio;
 
    return sizeRatio;
}
 
inline Eigen::MatrixXf circshift(const Eigen::MatrixXf &matrix, int dx, int dy)
{
    Eigen::MatrixXf matrix_out(matrix.rows(), matrix.cols());
    const int rows = matrix.rows();
    const int cols = matrix.cols();
    int idx_y = 0;
    int idx_x = 0;
    for(int i=0; i<rows; i++) {
        for(int j=0; j<cols; j++) {
            idx_y = modul(i+dy, rows);
            idx_x = modul(j+dx, cols);
            matrix_out(idx_y, idx_x) = matrix(i,j);
        }
    }
    return matrix_out;
}
 
inline Eigen::MatrixXf get2dGaussianPdf(const float sigma, const int w, const int h, const bool wrap_kernel = true)
{
    Eigen::MatrixXf y(h, w);
    float w2 = static_cast<float>(int(w/2));
    float h2 = static_cast<float>(int(h/2));
 
    for(int i=0; i<h; i++) {
        for(int j=0; j<w; j++) {
            y(i,j) = (float)exp((-0.5 / pow(sigma, 2)) * (pow((i-h2), 2) + pow((j-w2), 2)));
        }
    }
 
    if(wrap_kernel)
    {
        y = circshift(y, int(w/2), int(h/2));
    }
 
    return y;
}
 
inline const NvMotDataTypes::Rect2f unclipBbox(
    const NvMotDataTypes::Rect2f tBbox_new,
    const NvMotDataTypes::Rect2f srcImageBox, const NvMotDataTypes::Rect2f tBbox_prev
)
{
    Eigen::Vector2f min_new = tBbox_new.min();
    Eigen::Vector2f max_new = tBbox_new.max();
    const float w_new = tBbox_new.sizes()(0);
    const float h_new = tBbox_new.sizes()(1);
 
    const float w_prev = tBbox_prev.sizes()(0);
    const float h_prev = tBbox_prev.sizes()(1);
 
    const float w_src = srcImageBox.max()(0);
    const float h_src = srcImageBox.max()(1);
 
    if(w_prev > w_new && min_new(0) <= 1)
    {
        min_new(0) = max_new(0) - w_prev;
    }
 
    if(w_prev > w_new && max_new(0) > w_src - 1)
    {
        max_new(0) = min_new(0) + w_prev;
    }
 
    if(h_prev > h_new && min_new(1) <= 1)
    {
        min_new(1) = max_new(1) - h_prev;
    }
 
    if(h_prev > h_new && max_new(1) > h_src - 1)
    {
        max_new(1) = min_new(1) + h_prev;
    }
 
    return NvMotDataTypes::Rect2f(min_new, max_new);
}
 
inline const NvMotDataTypes::Rect2f computeSearchRegion(
    const NvMotDataTypes::Rect2f &bbox_new, const float paddingScale
)
{
    const Eigen::Vector2f min_new = bbox_new.min();
    const float newW = bbox_new.sizes()(0);
    const float newH = bbox_new.sizes()(1);
 
    Eigen::Vector2f centerPt;
 
    centerPt << min_new(0) + newW / 2.0f, min_new(1) + newH / 2.0f;
 
    float searchRegionSize_w = static_cast<float>(floorf(newW + paddingScale*sqrt(newW * newH)));
    float searchRegionSize_h = static_cast<float>(floorf(newH + paddingScale*sqrt(newW * newH)));
    searchRegionSize_w = (searchRegionSize_w + searchRegionSize_h) / 2.0f;
    searchRegionSize_h = searchRegionSize_w;
 
    NvMotDataTypes::Rect2f searchRegion = constructRect2f(
        centerPt(0) - searchRegionSize_w/2.f, centerPt(1) - searchRegionSize_h/2.f,
        searchRegionSize_w, searchRegionSize_h
    );
 
    return searchRegion;
}
 
inline float computeIOU(const NvMotDataTypes::Rect2f currBox, const NvMotDataTypes::Rect2f prevBox)
{
    const float areaPrev = prevBox.volume();
    const float areaCurr = currBox.volume();
 
    if(prevBox.isEmpty() || areaPrev < std::numeric_limits<float>::epsilon()
            || currBox.isEmpty() || areaCurr < std::numeric_limits<float>::epsilon())
        return 0;
 
    if(currBox.intersects(prevBox))
    {
        float intersection = currBox.intersection(prevBox).volume();
        float union_area = areaCurr + areaPrev - intersection;
 
        if(union_area < std::numeric_limits<float>::epsilon() || intersection < std::numeric_limits<float>::epsilon())
        {
            return 0;
        }
        else
        {
            return intersection / union_area;
        }
    }
    else
    {
        return 0;
    }
}
 
inline float computeVisibility(const NvMotDataTypes::Rect2f &objBbox, const NvMotDataTypes::Rect2f &imgBbox)
{
    if(imgBbox.isEmpty() || imgBbox.volume() < std::numeric_limits<float>::epsilon()
            || objBbox.isEmpty() || objBbox.volume() < std::numeric_limits<float>::epsilon())
        return 0;
 
    if(objBbox.intersects(imgBbox))
    {
        float intersection = objBbox.intersection(imgBbox).volume();
 
        if(intersection < std::numeric_limits<float>::epsilon())
        {
            return 0;
        }
        else
        {
            float iou = intersection / objBbox.volume();
 
            return iou;
        }
    }
    else
    {
        return 0;
    }
}
 
inline int orientation(const Eigen::Vector2d& p, const Eigen::Vector2d& q, const Eigen::Vector2d& r)
{
    float val = (q(1) - p(1)) * (r(0) - q(0)) -
              (q(0) - p(0)) * (r(1) - q(1));
 
    if (val == 0.) return 0;
    return (val > 0) ? 1 : 2;
}
 
inline int orientation(const Eigen::Vector2f& p, const Eigen::Vector2f& q, const Eigen::Vector2f& r)
{
    float val = (q(1) - p(1)) * (r(0) - q(0)) -
              (q(0) - p(0)) * (r(1) - q(1));
 
    if (val == 0.) return 0;
    return (val > 0) ? 1 : 2;
}
 
#ifdef VIZ_DEBUG
 
inline cv::Mat circshift(cv::Mat matrix, int dx, int dy)
{
    cv::Mat matrix_out = matrix.clone();
    int idx_y = 0;
    int idx_x = 0;
    for(int i=0; i<matrix.rows; i++) {
        for(int j=0; j<matrix.cols; j++) {
            idx_y = modul(i+dy, matrix.rows);
            idx_x = modul(j+dx, matrix.cols);
            matrix_out.at<float>(idx_y, idx_x) = matrix.at<float>(i,j);
        }
    }
    return matrix_out;
}
 
inline void OverlayImage(
    const cv::Mat &background, const cv::Mat &foreground, cv::Mat &output,
    cv::Point2i location = cv::Point(0,0), const cv::Mat &foregroundDensityMap = cv::Mat(),
    float opacity_min = 0, float opacity_offset = 1, float opacity_scale = 1
)
{
    if(background.cols != output.cols || background.rows != output.rows || background.type() != output.type() || background.channels() != output.channels())
        background.copyTo(output);
 
    for(int y = std::max(location.y , 0); y < background.rows; ++y)
    {
        int fY = y - location.y;
 
        if(fY >= foreground.rows)
            continue;
 
        for(int x = std::max(location.x, 0); x < background.cols; ++x)
        {
            int fX = x - location.x;
 
            {
                bool isFilled = false;
                for(int c = 0; c < output.channels(); ++c)
                    isFilled |= foreground.data[fY*foreground.step + fX*foreground.channels() + c];
                if(!isFilled)
                    continue;
            }
 
            if(fX >= foreground.cols)
                continue;
 
            double opacity_fg = 0;
 
            if(foregroundDensityMap.rows > 0 && foregroundDensityMap.at<float>(fY,fX) > opacity_min)
            {
                opacity_fg = std::max(
                    double(0),
                    std::min(double(1.), double(foregroundDensityMap.at<float>(fY,fX)*opacity_scale + opacity_offset))
                );
            }
            unsigned char foregroundPx, backgroundPx;
            for(int c = 0; opacity_fg > 0 && c < output.channels(); ++c)
            {
                foregroundPx = foreground.data[fY*foreground.step + fX*foreground.channels() + c];
                backgroundPx = background.data[y*background.step + x*background.channels() + c];
 
                output.data[y*output.step + output.channels()*x + c] = backgroundPx*(1.-opacity_fg) + foregroundPx*opacity_fg;
            }
        }
    }
}
 
inline cv::Point2f ConverPtBtwPlanes(const cv::Point2f &pt, const cv::Mat &projMat_A2B)
{
    cv::Mat pt_homo = cv::Mat(3,1,projMat_A2B.type());
    pt_homo.at<double>(0,0) = pt.x;
    pt_homo.at<double>(1,0) = pt.y;
    pt_homo.at<double>(2,0) = 1.;
 
    cv::Mat temp = projMat_A2B * pt_homo;
 
    cv::Point2f pt_transformed(temp.at<double>(0,0)/temp.at<double>(2,0), temp.at<double>(1,0)/temp.at<double>(2,0));
 
    return pt_transformed;
}
 
inline void Density2Opacity(const cv::Mat &input, cv::Mat &output)
{
 
}
 
inline bool warpPerspective_custom(const cv::Mat &inImg, cv::Mat &outImg, const cv::Mat &projMat_In2out)
{
    outImg = cv::Mat(inImg.cols, inImg.rows, inImg.type(), cv::Scalar(0));
 
    cv::Mat projMat_Out2In = projMat_In2out.inv();
 
    for(int y = 0; y < outImg.rows; ++y)
    {
        for(int x = 0; x < outImg.cols; ++x)
        {
            cv::Point2f pt_in = ConverPtBtwPlanes(cv::Point2f(x,y), projMat_Out2In);
 
            if(pt_in.x >= inImg.cols || pt_in.y >= inImg.rows || pt_in.x < 0 || pt_in.y < 0)
                continue;
 
            for(int c = 0; c < outImg.channels(); ++c)
            {
                outImg.data[y*outImg.step + x*outImg.channels() + c] = inImg.data[int(pt_in.y+0.5)*inImg.step + int(pt_in.x+0.5)*inImg.channels() + c];
            }
        }
    }
 
    return true;
}
 
inline bool getTransformedPixelDensity(const cv::Mat &inImg, cv::Mat &densityImg, const cv::Mat &projMat_In2out)
{
    densityImg = cv::Mat(inImg.rows, inImg.cols, CV_32F, cv::Scalar(0));
 
    uint STEP = 10;
 
    for(uint y = 0; y < densityImg.rows-STEP; y += STEP)
    {
        for(uint x = 0; x < densityImg.cols-STEP; x += STEP)
        {
            cv::Point2f pt_in_1 = ConverPtBtwPlanes(cv::Point2f(x,y), projMat_In2out);
            cv::Point2f pt_in_2 = ConverPtBtwPlanes(cv::Point2f(x+1,y+1), projMat_In2out);
            float dist_sq = pow(pt_in_1.x - pt_in_2.x,2) + pow(pt_in_1.y - pt_in_2.y,2);
            float density = 1./dist_sq;
 
            for(uint m=0; m<STEP; m++)
                for(uint n=0; n<STEP; n++)
                    densityImg.at<float>(y+m,x+n) = density;
        }
    }
    cv::normalize(densityImg, densityImg, 0, 1, cv::NORM_MINMAX, CV_32F);
 
    return true;
}
 
inline bool isFileExists(const std::string& filename)
{
    struct stat buf;
    if (stat(filename.c_str(), &buf) != -1)
    {
        return true;
    }
    return false;
}
 
inline cv::Mat NV12_to_BGR(const cv::Mat nv12_img)
{
    assert(nv12_img.channels() == 1);
 
    cv::Mat bgr_img;
    cv::cvtColor(nv12_img, bgr_img, cv::COLOR_YUV2BGR_NV12);
 
    return bgr_img;
}
 
inline cv::Mat NV12_to_BGR(unsigned char* yuv_data, unsigned int width, unsigned int height)
{
    cv::Mat nv12_img = cv::Mat(height * 3/2, width, CV_8UC1, yuv_data);
    cv::Mat bgr_img;
    cv::cvtColor(nv12_img, bgr_img, cv::COLOR_YUV2BGR_NV12);
 
    return bgr_img;
}
 
inline cv::Mat BGR_to_NV12(const cv::Mat src)
{
    assert(src.type() == CV_8UC3);
 
    cv::Mat dst = cv::Mat::zeros(cv::Size(src.size().width, src.size().height*1.5), CV_8UC1);
 
    for(int y=0; y<src.rows; y++)
    {
        for(int x=0; x<src.cols; x++)
        {
            const cv::Vec3b bgr = src.at<cv::Vec3b>(cv::Point(x,y));
            const uchar B = bgr.val[0];
            const uchar G = bgr.val[1];
            const uchar R = bgr.val[2];
 
            uchar &Y = dst.at<uchar>(cv::Point(x,y));
 
            Y = 0.257*R + 0.504*G + 0.098*B + 16 + 0.5f;
        }
    }
 
    for(int y=0; y<src.rows; y+=2)
    {
        for(int x=0; x<src.cols; x+=2)
        {
            const cv::Vec3b bgr = src.at<cv::Vec3b>(cv::Point(x,y));
            const uchar B = bgr.val[0];
            const uchar G = bgr.val[1];
            const uchar R = bgr.val[2];
 
            uchar &Cb = dst.at<uchar>(cv::Point(x+0,y/2+src.rows));
            uchar &Cr = dst.at<uchar>(cv::Point(x+1,y/2+src.rows));
 
            Cb = -0.148*R - 0.291*G + 0.439*B + 128;
            Cr =  0.439*R - 0.368*G - 0.071*B + 128;
        }
    }
 
    return dst;
}
 
inline cv::Mat BGR_to_NV12_2ch(const cv::Mat src)
{
    assert(src.type() == CV_8UC3);
 
    std::vector<cv::Mat> dst_per_ch(2);
    dst_per_ch[0] = cv::Mat::zeros(src.size(), CV_8UC1);
    dst_per_ch[1] = cv::Mat::zeros(src.size(), CV_8UC1);
 
    for(int y=0; y<src.rows; y++)
    {
        for(int x=0; x<src.cols; x++)
        {
            const cv::Vec3b bgr = src.at<cv::Vec3b>(cv::Point(x,y));
            const uchar B = bgr.val[0];
            const uchar G = bgr.val[1];
            const uchar R = bgr.val[2];
 
            uchar &Y = dst_per_ch[0].at<uchar>(cv::Point(x,y));
 
            Y = 0.257*R + 0.504*G + 0.098*B + 16 + 0.5f;
        }
    }
 
    for(int y=0; y<src.rows; y+=2)
    {
        for(int x=0; x<src.cols; x+=2)
        {
            const cv::Vec3b bgr = src.at<cv::Vec3b>(cv::Point(x,y));
            const uchar B = bgr.val[0];
            const uchar G = bgr.val[1];
            const uchar R = bgr.val[2];
 
            uchar &Cb = dst_per_ch[1].at<uchar>(cv::Point(x+0,y/2));
            uchar &Cr = dst_per_ch[1].at<uchar>(cv::Point(x+1,y/2));
 
            Cb = -0.148*R - 0.291*G + 0.439*B + 128;
            Cr =  0.439*R - 0.368*G - 0.071*B + 128;
        }
    }
 
    cv::Mat dst;
 
    cv::merge(dst_per_ch, dst);
 
    return dst;
}
 
inline cv::Mat NV12_2ch_to_BGR(cv::Mat src)
{
    assert(src.channels() == 2);
 
    std::vector<cv::Mat> nv12_per_ch;
 
    cv::split(src, nv12_per_ch);
 
    cv::Mat nv12_img = cv::Mat::zeros(cv::Size(src.size().width, src.size().height*3/2), CV_8UC1);
 
    nv12_per_ch[0].copyTo(nv12_img( cv::Rect(0,0,src.size().width,src.size().height) ));
 
    nv12_per_ch[1](
            cv::Rect(0,0,src.size().width,src.size().height*0.5)
            ).copyTo(nv12_img( cv::Rect(0,src.size().height,src.size().width,src.size().height*0.5) ));
 
    cv::Mat bgr_img;
    cv::cvtColor(nv12_img, bgr_img, cv::COLOR_YUV2BGR_NV12);
 
    return bgr_img;
}
 
#endif
 
} // end of CV_FUNCS
 
#endif // CV_FUNCTIONS_HPP