CleanRobot-UESTC/融合/ranging.h

#pragma once

#include <opencv2/opencv.hpp>
#include <opencv2/calib3d.hpp>

#include <cstdlib>
#include <vector>
#include "rknn_sdk.h"
#include "fusion.h"

using namespace cv;
class LCFusion;
class Ranging
{
private:
    friend class LCFusion;
    VideoCapture vcapture;
    rknn_handle hdx;
    const char *modelPath = "m28.rknn";
    std::vector<std::string> className = {
        "Unknown",      //未知
        "MaoGouWan",    //猫碗/狗碗
        "BTYDiZuo",     //吧台怅底座
        "FSDiZuo",      //风扇底座
        "XiYiJi",       //掗衣机
        "BingXiang",    //冰箱
        "MaTong",       //马桶
        "TiZhongCheng", //䜓重秀
        "DianXian",     //电线
        "DianShiJi",    //电视机
        "CanZhuo",      //逐桌
        "DiTan",        //地毯
        "MaBu",         //抹垃
        "ChaJi",        //茶几
        "DianShiGu",    //电视柜
        "XieZi",        //拖鞋/鞋子
        "WaZi",         //袜子
        "YiGui",        //衣柜
        "Chuang",       //床
        "ShaFa",        //沙发
        "YiZi",         //怅子
        "Men",          //闹
        "DLS",          //倧理石
        "DiBan",        //地板
    };

    // 双目参数
   Mat cam_matrix_left = (Mat_<double>(3, 3) << 
        4.700950170847520e+02, 0, 0,
        0, 4.709244004725060e+02, 0,
        3.220628601465352e+02, 2.265265667516499e+02, 1);

    Mat cam_matrix_right = (Mat_<double>(3, 3) << 
        4.696016526986375e+02, 0, 0,
        0, 4.702371464881610e+02, 0,
        3.216994731643572e+02, 2.267449827655198e+02, 1);
    Mat distortion_l = (Mat_<double>(1, 5) << 0.088023418049400, -0.047968406803285, 0,
        0, 0);

    Mat distortion_r = (Mat_<double>(1, 5) << 0.097018738959281, -0.081707209740704, 0,
        0, 0);

    Mat rotate = (Mat_<double>(3, 3) << 
        0.999895538560624, 1.514179660491009e-04, 0.014452994124399,
        -1.935031776135467e-04, 0.999995745723263, 0.002910514026079,
        -0.014452491933249, -0.002913006689884, 0.999891314028152);
    Mat trans = (Mat_<double>(3, 1) << 
        -59.978995009996730, 0.050886852679934, -0.088565378305175);
/*

    Mat cam_matrix_left = (Mat_<double>(3, 3) << 
        3.627346593660044e+02, 0, 0,
        0, 3.629168542317134e+02, 0,
        3.093231127433517e+02, 2.409353074183058e+02, 1);

    Mat cam_matrix_right = (Mat_<double>(3, 3) << 
        3.615909129180773e+02, 0, 0,
        0, 3.617772267770084e+02, 0,
        3.062363746645480e+02, 2.298948842159400e+02, 1);
    Mat distortion_l = (Mat_<double>(1, 5) << 0.117219440226075, -0.148594691012994, 0,
        0, 0);
    Mat distortion_r = (Mat_<double>(1, 5) << 0.116412295415583, -0.146817143572705, 0,
        0, 0);
    Mat rotate = (Mat_<double>(3, 3) << 
        0.999962353120719, 4.648106081008926e-04, 0.008664657660520,
        -4.493098874759400e-04, 0.999998295540464, -0.001790820145135,
        -0.008665475284162, 0.001786859609987, 0.999960857569352);
    Mat trans = (Mat_<double>(3, 1) << 
        -60.097178758944190, -0.033859553542635, -0.423965574285253);
*/
/*
    Mat cam_matrix_left = (Mat_<double>(3, 3) << 3.629995979572968e+02, 0, 0, 0, 3.605811984929196e+02, 0, 3.056618479641253e+02, 2.425978482096805e+02, 1);
    Mat cam_matrix_right = (Mat_<double>(3, 3) << 3.642383320232306e+02, 0, 0, 0, 3.619176322068948e+02, 0, 3.048699917391443e+02, 2.355042537698953e+02, 1);
    Mat distortion_l = (Mat_<double>(1, 5) << 0.114519742751324, -0.147768779338520, 0.001196745401736, -0.00274469994995, 0.008589264227682);
    Mat distortion_r = (Mat_<double>(1, 5) << 0.107862414325132, -0.112272107836791, 0.002046119288761, -8.703752362970271e-04, -0.050706452190122);
    Mat rotate = (Mat_<double>(3, 3) << 0.999984364022271, 6.994668898371339e-04, 0.005548194034452, -6.988446369697146e-04, 0.999999749299562, -1.140920137281147e-04, -0.005548272447104, 1.102129041420978e-04, 0.999984602144437);
    Mat trans = (Mat_<double>(3, 1) << -60.134851044411256, 0.007279986569326, -0.065092184396760);
*/

    Mat mapX1, mapX2, mapY1, mapY2, q, Z;

    int imgw, imgh;

public:
    Ranging(int index, int imgw, int imgh);
    void rectifyImage(Mat &oriImgL, Mat &oriImgR);
    void getInfo(Mat &imgl, Mat &imgr, Mat &detBoxes, Mat &info);
    std::vector<float> pic2cam(int u, int v);
    std::vector<int> muban(Mat &left_image, Mat &right_image, const int *coordinates);
    std::vector<Mat> get_range();
    void horizon_estimate(Mat& img, Mat& bboxs,int k);
};