2020-08-05

最新推荐文章于 2021-08-19 13:59:17 发布
最新推荐文章于 2021-08-19 13:59:17 发布
阅读量180 收藏
点赞数
分类专栏： QT
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_37732417/article/details/107817184
版权

#include <iostream>
#include "XMatLib.h"
#include "alg.h"
#include "math.h"
#include "uwb_util.h"
#include <windows.h>
#include <sql.h>  
#include <sqlext.h>   
#include <odbcss.h>  


#define MAP202_flag 0
#define MapXmin 0
#define MapXmax 623
#define MapYmin 0
#define MapYmax 691

 

 

using namespace std;
const uint64_t c_Light_Speed = 29979245800;        //Speed of light c  cm/s 
const uint64_t CLOCK_COUNTS = 63897600000;        //1 second in internal clock counts
uint32_t target_Id[10] = { 0 };
uint32_t Device_ID = 0;
double a[4][3] = { {0,0,0} ,{0,0,0} ,{0,0,0} ,{0,0,0} };
TGlobalCfg g_GlobalConfig;
ALG_LOCADATA alg_data;
void Init_anchor(void);
void get_pos(uint64_t ts_1, uint64_t ts_2, uint64_t ts_3, uint64_t ts_4);
void get_pos_temp(uint64_t ts_1, uint64_t ts_2, uint64_t ts_3, uint64_t ts_4);
double BS[4][3] = {
    { 490, 10 ,253},
    { 124, 10 ,253},
    { 120, 693,255 },
    { 490, 693 ,267}
};
/***************************************************************************************************************
* NOTE      |
* ParamIn   |
* ParamOut  |
* Return    |
***************************************************************************************************************/

int main()
{
    double orginData[4];
    char qBuff[100] = { 0 };
    system("mode con cols=150 lines=50");
    HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE);
    SetConsoleTitleW(L"UWB定位引擎");

    SQLHENV henv = SQL_NULL_HENV;
    SQLHANDLE dbc;
    const char dsn[] = "DRIVER={SQL Server};SERVER=19.87.3.157;DATABASE=zhao;UID=sa;PWD=ZCX213526";

    //TODO 
    RETCODE retcode = SQLAllocHandle(SQL_HANDLE_ENV, NULL, &henv);
    retcode = SQLSetEnvAttr(henv, SQL_ATTR_ODBC_VERSION, (SQLPOINTER)SQL_OV_ODBC3, SQL_IS_INTEGER);
    retcode = SQLAllocHandle(SQL_HANDLE_DBC, henv, &dbc);

    SQLCHAR outstr[1024];
    SQLSMALLINT outstrlen;
    retcode = SQLDriverConnect(dbc, NULL, (SQLCHAR*)dsn, SQL_NTS, outstr, sizeof(outstr), &outstrlen, SQL_DRIVER_NOPROMPT);
    if ((retcode != SQL_SUCCESS) && (retcode != SQL_SUCCESS_WITH_INFO))
    {
        printf("连接失败!\n");
        system("PAUSE");
        return -1;
    }

    SQLHSTMT stmt;
    int intTemp = SQLAllocHandle(SQL_HANDLE_STMT, dbc, &stmt);
    if (SQL_SUCCESS != SQLAllocHandle(SQL_HANDLE_STMT, dbc, &stmt))
    {
        printf("Failed to connect");
        system("PAUSE");
        return -1;
    }


    bool flag = true;
    retcode = SQLAllocHandle(SQL_HANDLE_STMT, dbc, &stmt);
    retcode = SQLSetStmtAttr(stmt, SQL_ATTR_ROW_BIND_TYPE, (SQLPOINTER)SQL_BIND_BY_COLUMN, SQL_IS_INTEGER);
    while (true)
    {
        double Pos[2] = { 0 };
        double x = 0;
        double y = 0;
        SQLLEN timeStamp[4] = { 0 };
        SQLCHAR time[32], pos[32];
        SQLINTEGER target, loc_sn;
        SQLLEN mLen;

        retcode = SQLExecDirect(stmt, (SQLCHAR*)("SELECT  TOP 1 * FROM uwb_tb  WHERE pos IS NULL AND anchor_a IS NOT NULL AND anchor_b IS NOT NULL AND anchor_c IS NOT NULL AND anchor_d IS NOT NULL ORDER BY loc_sn DESC"), SQL_NTS);
        if (retcode == SQL_SUCCESS || retcode == SQL_SUCCESS_WITH_INFO)
        {
            /*  句柄、列、变量类型、接收缓冲、缓冲长度、返回的长度   */
            SQLBindCol(stmt, 1, SQL_C_LONG, &target, 4, &mLen);
            SQLBindCol(stmt, 2, SQL_C_LONG, &loc_sn, 4, &mLen);
            SQLBindCol(stmt, 3, SQL_C_SBIGINT, &timeStamp[0], 8, &mLen);
            SQLBindCol(stmt, 4, SQL_C_SBIGINT, &timeStamp[1], 8, &mLen);
            SQLBindCol(stmt, 5, SQL_C_SBIGINT, &timeStamp[2], 8, &mLen);
            SQLBindCol(stmt, 6, SQL_C_SBIGINT, &timeStamp[3], 8, &mLen);
            SQLBindCol(stmt, 7, SQL_C_CHAR, time, 32, &mLen);
        }
        else if (retcode == SQL_NO_DATA)
        {
            system("PAUSE");
            continue;
        }

        if (true == flag)
        {
            SetConsoleTextAttribute(hOut, FOREGROUND_GREEN);
            flag = !flag;
        }
        else
        {
            SetConsoleTextAttribute(hOut, FOREGROUND_INTENSITY);
            flag = !flag;
        }
        //printf("into while get pos !!!!!!!!!!!!!!!!!!!!!!!!!!\n\n");
        //遍历数据  


        while (true)
        {
            retcode = SQLFetch(stmt);
            if (retcode == SQL_NO_DATA_FOUND)
            {
                break;
            }
            else if (retcode == SQL_ERROR)
            {
                printf("fetch error\n");
                break;
            }
            else
            {
                
                Device_ID = (uint32_t)target;
                Init_anchor();
                get_pos(timeStamp[0], timeStamp[1], timeStamp[2], timeStamp[3]);
                get_pos_temp(timeStamp[0], timeStamp[1], timeStamp[2], timeStamp[3]);

                printf("%s\t %d\t %d\t %llu\t %llu\t %llu\t %llu\t %f\t%f\t\n\n", time, target, loc_sn, timeStamp[0], timeStamp[1], timeStamp[2], timeStamp[3],alg_data.tagInfo.Mat_PosX, alg_data.tagInfo.Mat_PosY);
                //get_pos_temp(timeStamp[0], timeStamp[1], timeStamp[2], timeStamp[3]);
            }
        }
        SQLFreeHandle(SQL_HANDLE_STMT, stmt);
        retcode = SQLAllocHandle(SQL_HANDLE_STMT, dbc, &stmt);

            sprintf(qBuff, "UPDATE uwb_tb SET pos=\'%f,%f\' WHERE target=%d AND loc_sn=%d", a[3][0], a[3][1], target, loc_sn);

            retcode = SQLExecDirect(stmt, (SQLCHAR*)qBuff, SQL_NTS);
            if (retcode != SQL_SUCCESS)
            {
                printf("[%d]>>%s\n", retcode, qBuff);
            }
        

        memset(qBuff, 0, sizeof(qBuff));

    }

    SQLFreeHandle(SQL_HANDLE_STMT, stmt);
    SQLDisconnect(dbc);
    SQLFreeHandle(SQL_HANDLE_DBC, dbc);
    SQLFreeHandle(SQL_HANDLE_ENV, henv);

    system("PAUSE");
    return 0;

}
/***************************************************************************************************************
* NOTE      |
* ParamIn   |
* ParamOut  |
* Return    |
***************************************************************************************************************/
#if MAP202_flag
void Init_anchor(void)
{
    g_GlobalConfig.select_count = 5;

    memset(&alg_data, 0, sizeof(alg_data));
    alg_data.tagInfo.TagId = Device_ID;
    alg_data.tagInfo.BaseStationNum = 4;
    alg_data.tagInfo.loca_mode = 1;
    alg_data.tagInfo.dimension = 2;
    alg_data.tagInfo.fPresetHeight = 50.0;//80.0;//150
    alg_data.tagInfo.qCurTime = _Xtime_get_ticks() / 10000;
    alg_data.tagInfo.b18 = 0x52;
    alg_data.tagInfo.b19 = 3;
    alg_data.tagInfo.w1A = 0x44C;
    alg_data.tagInfo.qw8 = 3;
    alg_data.tagInfo.u38 = 2;
    alg_data.tagInfo.MapLevel = 1;

    alg_data.anchor_data[0].AnchorId = 0x20070018050392;
    //alg_data.anchor_data[0].timestamp = 0xE77B55010B;
    //alg_data.anchor_data[0].timestamp2 = alg_data.anchor_data[0].timestamp;
    alg_data.anchor_data[0].FirstSignalLv = -83.23f;
    //alg_data.anchor_data[0].FirstSignalLv = -10.23f;
    alg_data.anchor_data[0].SignalLv = -79.21f;
    alg_data.anchor_data[0].PosX = 62;//485
    alg_data.anchor_data[0].PosY = 261.0f;//10
    alg_data.anchor_data[0].PosZ = 370;//245

    alg_data.anchor_data[1].AnchorId = 0x20070018050469;
    //alg_data.anchor_data[1].timestamp = 0xE77B550104;
    //alg_data.anchor_data[1].timestamp2 = alg_data.anchor_data[1].timestamp;
    alg_data.anchor_data[1].FirstSignalLv = -87.43;
    //alg_data.anchor_data[0].FirstSignalLv = -10.23f;
    alg_data.anchor_data[1].SignalLv = -79.67;
    alg_data.anchor_data[1].PosX = 669;//109
    alg_data.anchor_data[1].PosY = 207.0f;//10
    alg_data.anchor_data[1].PosZ = 339;

    alg_data.anchor_data[2].AnchorId = 0x20070018050282;
    //alg_data.anchor_data[2].timestamp = 0xE77B54FFE5;
    //alg_data.anchor_data[2].timestamp2 = alg_data.anchor_data[2].timestamp;
    alg_data.anchor_data[2].FirstSignalLv = -95.28;
    //alg_data.anchor_data[0].FirstSignalLv = -10.23f;
    alg_data.anchor_data[2].SignalLv = -80.53;
    alg_data.anchor_data[2].PosX = 709;//515
    alg_data.anchor_data[2].PosY = 1097.0f;//609
    alg_data.anchor_data[2].PosZ = 340;//245


    alg_data.anchor_data[3].AnchorId = 0x20070018050464;
    //alg_data.anchor_data[3].timestamp = 0xE77B550015;
    //alg_data.anchor_data[0].FirstSignalLv = -10.23f;
    alg_data.anchor_data[3].SignalLv = -76.57;
    alg_data.anchor_data[3].PosX = 57;//119
    alg_data.anchor_data[3].PosY = 1097.0f;//690.0f;
    alg_data.anchor_data[3].PosZ = 340;//258

    for (uint i = 0; i < alg_data.tagInfo.BaseStationNum; i++)
    {
        alg_data.anchor_data[i].bUse50 = 1;
        alg_data.anchor_data[i].has_tdoa = 1;
        alg_data.anchor_data[i].qw8 = 1;
    }
}
#else
void Init_anchor(void)
{
    g_GlobalConfig.select_count = 5;

    memset(&alg_data, 0, sizeof(alg_data));
    alg_data.tagInfo.TagId = Device_ID;
    alg_data.tagInfo.BaseStationNum = 4;
    alg_data.tagInfo.loca_mode = 1;
    alg_data.tagInfo.dimension = 2;
    alg_data.tagInfo.fPresetHeight = 80.0;//80.0;//150
    alg_data.tagInfo.qCurTime = _Xtime_get_ticks() / 10000;
    alg_data.tagInfo.b18 = 0x52;
    alg_data.tagInfo.b19 = 23;//3
    alg_data.tagInfo.w1A = 0x44C;
    alg_data.tagInfo.qw8 = 3;
    alg_data.tagInfo.u38 = 2;
    alg_data.tagInfo.MapLevel = 1;

    alg_data.anchor_data[0].AnchorId = 0x20070018050392;
    //alg_data.anchor_data[0].timestamp = 0xE77B55010B;
    //alg_data.anchor_data[0].timestamp2 = alg_data.anchor_data[0].timestamp;
    alg_data.anchor_data[0].FirstSignalLv = -83.23f;
    //alg_data.anchor_data[0].FirstSignalLv = -10.23f;
    alg_data.anchor_data[0].SignalLv = -79.21f;
    alg_data.anchor_data[0].PosX = 490;//485
    alg_data.anchor_data[0].PosY = 10.0f;//10
    alg_data.anchor_data[0].PosZ = 253;//245

    alg_data.anchor_data[1].AnchorId = 0x20070018050469;
    //alg_data.anchor_data[1].timestamp = 0xE77B550104;
    //alg_data.anchor_data[1].timestamp2 = alg_data.anchor_data[1].timestamp;
    alg_data.anchor_data[1].FirstSignalLv = -87.43;
    //alg_data.anchor_data[0].FirstSignalLv = -10.23f;
    alg_data.anchor_data[1].SignalLv = -79.67;
    alg_data.anchor_data[1].PosX = 124;//109
    alg_data.anchor_data[1].PosY = 10.0f;//10
    alg_data.anchor_data[1].PosZ = 253;

    alg_data.anchor_data[2].AnchorId = 0x20070018050282;
    //alg_data.anchor_data[2].timestamp = 0xE77B54FFE5;
    //alg_data.anchor_data[2].timestamp2 = alg_data.anchor_data[2].timestamp;
    alg_data.anchor_data[2].FirstSignalLv = -95.28;
    //alg_data.anchor_data[0].FirstSignalLv = -10.23f;
    alg_data.anchor_data[2].SignalLv = -80.53;
    alg_data.anchor_data[2].PosX = 120;//515
    alg_data.anchor_data[2].PosY = 693.0f;//609
    alg_data.anchor_data[2].PosZ = 255;//245


    alg_data.anchor_data[3].AnchorId = 0x20070018050464;
    //alg_data.anchor_data[3].timestamp = 0xE77B550015;
    //alg_data.anchor_data[0].FirstSignalLv = -10.23f;
    alg_data.anchor_data[3].SignalLv = -76.57;
    alg_data.anchor_data[3].PosX = 490;//119
    alg_data.anchor_data[3].PosY = 693.0f;//690.0f;
    alg_data.anchor_data[3].PosZ = 267;//258

    for (uint i = 0; i < alg_data.tagInfo.BaseStationNum; i++)
    {
        alg_data.anchor_data[i].bUse50 = 1;
        alg_data.anchor_data[i].has_tdoa = 1;
        alg_data.anchor_data[i].qw8 = 1;
    }
}
#endif
/***************************************************************************************************************
* NOTE      |
* ParamIn   |
* ParamOut  |
* Return    |
***************************************************************************************************************/
bool comp_timestamp(const TAnchorData& a, const TAnchorData& b)
{
    return (int64)b.timestamp2 > (int64)a.timestamp;
}
/***************************************************************************************************************
* NOTE      |
* ParamIn   |
* ParamOut  |
* Return    |
***************************************************************************************************************/
bool comp_tdoa(const TAnchorData& a, const TAnchorData& b)
{
    return b.tdoa > a.tdoa;
}
/***************************************************************************************************************
* NOTE      |
* ParamIn   |
* ParamOut  |
* Return    |
***************************************************************************************************************/
int CalcTdoa(ALG_LOCADATA* pLocData)
{
    int ret = 0;

    if (g_GlobalConfig.select_count < 4)
    {
        pLocData->tagInfo.err_94 = 14;
        return 0;
    }
    arma::Mat<double> locMat1(pLocData->tagInfo.BaseStationNum, 5);
    locMat1.zeros();

    std::sort(pLocData->anchor_data,
        &pLocData->anchor_data[pLocData->tagInfo.BaseStationNum], comp_timestamp);

    uint UseCount = 0;

    for (uint i = 0; i < pLocData->tagInfo.BaseStationNum; i++)
    {
        TAnchorData* pAnchor = &pLocData->anchor_data[i];
        (*(arma::Mat<uint64>*) & locMat1)(i, 0) = pAnchor->AnchorId;
        locMat1(i, 1) = pAnchor->PosX;
        locMat1(i, 2) = pAnchor->PosY;
        locMat1(i, 3) = pAnchor->PosZ;
        (*(arma::Mat<uint64>*) & locMat1)(i, 4) = pAnchor->timestamp2;
        UseCount++;
    }

    if (UseCount < 4)
    {
        pLocData->tagInfo.err_94 = 50;
        return 0;
    }

    //locMat1.print();
    //cout << "subview=======" << endl;

    uint start_row = 0;
    for (start_row = 1; start_row <= (UseCount - 3); start_row++)
    {
        arma::subview<double> sv1 = locMat1.submat(0, 1, 0, 2);
        arma::subview<double> sv2 = locMat1.submat(1, 1, 1, 2);
        arma::subview<double> sv3 = locMat1.submat(0, 1, 0, 2);
        arma::subview<double> sv4 = locMat1.submat(2, 1, 2, 2);

        Glue_sv_sv_join GlueA(sv1, sv2);
        Glue_sv_sv_join GlueB(sv3, sv4);
        arma::Glue<Glue_sv_sv_join, Glue_sv_sv_join, arma::glue_join_cols> glue_glue_mat(GlueA, GlueB);

        //求行列式
        double detv = XMatLib::det_glue(glue_glue_mat);
        if (detv != 0.0f)
            break;

        arma::subview_row<double> sv_row1 = locMat1.row(2);
        arma::subview_row<double> sv_rowi = locMat1.row(2 + start_row);

        arma::subview_row<double> sv_tmprow = locMat1.row(2);
        sv_tmprow = sv_rowi;
        sv_tmprow = locMat1.row(2 + start_row);
        sv_tmprow = sv_row1;
    }
    if (start_row > (UseCount - 3))
        return 0;

    arma::Mat<double> tmpMat1(0, 0);
    uint i_row = 4;
    for (start_row = 3; i_row <= UseCount; start_row++)
    {
        double row3i_x = locMat1(start_row, 1);
        double row3i_y = locMat1(start_row, 2);
        double row0_x = locMat1(0, 1);
        double row0_y = locMat1(0, 2);
        double row1_x = locMat1(1, 1);
        double row1_y = locMat1(1, 2);
        double row2_x = locMat1(2, 1);
        double row2_y = locMat1(2, 2);

        //求出前三个基站与最后一个基站的坐标距离
        double dis0_3i = std::sqrt((row0_x - row3i_x) * (row0_x - row3i_x) + (row0_y - row3i_y) * (row0_y - row3i_y));
        double dis1_3i = std::sqrt((row1_x - row3i_x) * (row1_x - row3i_x) + (row1_y - row3i_y) * (row1_y - row3i_y));
        double dis2_3i = std::sqrt((row2_x - row3i_x) * (row2_x - row3i_x) + (row2_y - row3i_y) * (row2_y - row3i_y));
        //cout << dis0_3i << "  " << dis1_3i << "  " << dis2_3i << endl;
        if (dis1_3i >= dis0_3i && dis2_3i >= dis1_3i)
            break;

        i_row++;
    }
    if (i_row > UseCount)
    {
        tmpMat1 = locMat1.row(3);
        i_row = 4;
    }
    else
    {
        tmpMat1 = locMat1.row(i_row - 1);
    }

    arma::Mat<double> tmpMat2(0, 0);
    int select_count = g_GlobalConfig.select_count;
    if (g_GlobalConfig.select_count >= 5 && UseCount > 4)
    {
        if (UseCount < g_GlobalConfig.select_count)
            select_count = UseCount;
        locMat1.shed_row(i_row - 1);
        tmpMat2 = locMat1.rows(3, select_count - 2);
    }

    arma::subview<double> sv_tmprow = tmpMat1.row(0);
    arma::subview<double> sv_row1 = locMat1.rows(0, 2);

    //arma::mat tmpMat3(0, 0);
    //arma::Glue<arma::subview<double>, arma::subview<double>, arma::glue_join_cols> glue_svsv(sv_row1, sv_tmprow);
    //arma::Glue<Glue_sv_sv_join, arma::mat, arma::glue_join_cols> glue_glue_mat(glue_svsv, tmpMat2);
    //arma::glue_join_cols::apply(tmpMat3, glue_glue_mat);

    arma::mat tmpMat3 = arma::join_cols(sv_row1, sv_tmprow, tmpMat2);

    int tMat3_n_rows = tmpMat3.n_rows;
    if (tmpMat3.n_rows)
    {
        uint64_t timestamp0 = (*(arma::Mat<uint64>*) & tmpMat3)(0, 4);
        for (int i = 0; i < tmpMat3.n_rows; i++)
        {
            // timespan / CLOCK_COUNTS 为 TDOA
            // TDOA 值乘以光速，就是标签到两个基站距离差的观测值
            uint64_t timestamp_tmp = (*(arma::Mat<uint64>*) & tmpMat3)(i, 4);
            double timespan = (double)((uint32_t)timestamp_tmp - (uint32_t)timestamp0);
            tmpMat3(i, 4) = timespan / CLOCK_COUNTS * c_Light_Speed;
        }
    }
    else
    {
        tMat3_n_rows = 0;
    }

    for (uint i = 0; i < pLocData->tagInfo.BaseStationNum; i++)
    {
        for (int m = 0; m < tMat3_n_rows; m++)
        {
            if ((*(arma::Mat<uint64>*) & tmpMat3)(m, 0) == pLocData->anchor_data[i].AnchorId)
            {
                pLocData->anchor_data[i].tdoa = tmpMat3(m, 4);
                pLocData->anchor_data[i].has_tdoa = 1;
                break;
            }
        }
    }

    std::sort(pLocData->anchor_data,
        &pLocData->anchor_data[pLocData->tagInfo.BaseStationNum], comp_tdoa);
    ret = 1;
    return ret;
}
/***************************************************************************************************************
* NOTE      |
* ParamIn   |
* ParamOut  |
* Return    |
***************************************************************************************************************/
int CalcM3(std::vector<double>& vecPosX, std::vector<double>& vecPosY, std::vector<double>& vecPosZ, std::vector<double>& vecR, Loc12_Node_Item* pTagPos, char b18, double factor, double fPresetHeight)
{
    int ret = 0;
    int tdoa_num = vecPosX.size() - 1;
    arma::mat locMatX(vecPosX.size(), 1);
    arma::mat locMatY(vecPosX.size(), 1);
    arma::mat locMatZ(vecPosX.size(), 1);
    arma::mat locMatR(vecPosX.size() - 1, 1);

    memcpy((double*)locMatX.mem, vecPosX.data(), sizeof(double) * vecPosX.size());
    memcpy((double*)locMatY.mem, vecPosY.data(), sizeof(double) * vecPosX.size());
    memcpy((double*)locMatZ.mem, vecPosZ.data(), sizeof(double) * vecPosX.size());
    memcpy((double*)locMatR.mem, vecR.data() + 1, sizeof(double) * tdoa_num);

    arma::mat DisMat(4, 1);
    arma::mat AverageMat(1, 2);
    AverageMat(0, 0) = pTagPos->average_X;
    AverageMat(0, 1) = pTagPos->average_Y;
    DisMat(0, 0) = pTagPos->min_posX;
    DisMat(1, 0) = pTagPos->max_posX;
    DisMat(2, 0) = pTagPos->min_posY;
    DisMat(3, 0) = pTagPos->max_posY;

    int Span_b18 = b18 - pTagPos->b18;
    if (Span_b18 < 0)
        Span_b18 += 0x100;

    double b18_factor = Span_b18 / factor;
    arma::Mat<double> tmpMat1(tdoa_num, tdoa_num);
    tmpMat1.fill(200.0);
    arma::mat matD1 = arma::diagmat(tmpMat1);
    tmpMat1 += matD1;

    arma::Mat<double> tmpMat2(2, 2);
    tmpMat2.fill(2.0);
    arma::mat matD2 = arma::diagmat(tmpMat2);
    tmpMat2 = matD2;

    matD1 = arma::inv(tmpMat1);
    matD2 = arma::inv(tmpMat2);

    double svd_max1 = arma::norm(tmpMat1, 2);
    double svd_max2 = arma::norm(tmpMat2, 2);

    double fen1 = 1.0 / std::sqrt(std::pow(6.28, tdoa_num) * svd_max1);
    double fen2 = 1.0 / std::sqrt(std::pow(6.28, 2.0) * svd_max2);

    arma::Mat<double> tmpMat3(1, 2);
    tmpMat3.fill(0.0);

    double tmpb18_200 = b18_factor * 200.0;
    if (factor <= 20.0)
    {
        DisMat(0, 0) -= tmpb18_200;
        DisMat(1, 0) += tmpb18_200;
        DisMat(2, 0) -= tmpb18_200;
        DisMat(3, 0) += tmpb18_200;
    }
    else
    {
        DisMat(0, 0) -= tmpb18_200 + 100.0;
        DisMat(1, 0) += tmpb18_200 + 100.0;
        DisMat(2, 0) -= tmpb18_200 + 100.0;
        DisMat(3, 0) += tmpb18_200 + 100.0;
    }

    arma::mat tmpMat_d1(2, 1);
    arma::mat tmpMat_d2(1, 2);
    arma::mat tmpMat_d3(1, 2);
    tmpMat_d1.row(0) = DisMat.row(0);
    tmpMat_d1.row(1) = DisMat.row(2);
    double tmpd1 = DisMat(1, 0) - DisMat(0, 0);
    double tmpd2 = DisMat(3, 0) - DisMat(2, 0);

    int tmp1 = (int)(tmpd1 / 20.0 + 1.0 + 0.5);
    int tmp2 = (int)(tmpd2 / 20.0 + 1.0 + 0.5);
    int tmp1x2 = tmp1 * tmp2;
    if (tmp1x2 <= 0)
        return 0;

    static int LastFix = 0;
    int tmpFix = LastFix;
    if (!LastFix)
    {
        tmpFix = 2 * tmp1x2;
        LastFix = tmpFix;
    }
    if (tmp1x2 > tmpFix)
        return 0;

    arma::mat locMatX1(tmp1x2, 1);
    arma::mat locMatY1(tmp1x2, 1);
    for (int i = 0; i < tmp2; i++)
    {
        for (int m = 0; m < tmp1; m++)
        {
            locMatX1(i * tmp1 + m, 0) = tmpMat_d1(0, 0) + m * 20;
            locMatY1(i * tmp1 + m, 0) = tmpMat_d1(1, 0) + i * 20;
        }
    }
    arma::mat locMatX3(tmp1x2, 1);
    for (int i = 0; i < tmp1x2; i++)
    {
        locMatX3(i, 0) = 1.0 / (double)tmp1x2;
    }

    arma::mat tmpOutMat1(tdoa_num, tmp1x2);
    arma::mat tmpOutMat2(tmp1x2, 1);
    arma::mat tmpOutMat3(tmp1x2, 1);
    arma::mat tmpOutMat4(tmp1x2, 1);

    for (int i = 0; i < tmp1x2; i++)
    {
        arma::mat tmp_MatX = (locMatX.row(0) - locMatX1.row(i)) * (locMatX.row(0) - locMatX1.row(i));
        arma::mat tmp_MatY = (locMatY.row(0) - locMatY1.row(i)) * (locMatY.row(0) - locMatY1.row(i));
        arma::mat tmp_MatZ = (locMatZ.row(0) - fPresetHeight) * (locMatZ.row(0) - fPresetHeight);

        arma::mat tmpResult = tmp_MatX + tmp_MatY + tmp_MatZ;
        arma::mat mat_sqrt = arma::sqrt(tmpResult);
        tmpOutMat2.row(i) = mat_sqrt;
    }
    arma::mat tmpOutMat5(1, 1);
    for (int i = 0; i < tmp1x2; i++)
    {
        arma::mat tmp = locMatX.rows(1, tdoa_num) - locMatX1(i, 0);
        arma::mat tmp_MatX = arma::pow(tmp, 2);

        tmp = locMatY.rows(1, tdoa_num) - locMatY1(i, 0);
        arma::mat tmp_MatY = arma::pow(tmp, 2);

        tmp = locMatZ.rows(1, tdoa_num) - fPresetHeight;
        arma::mat tmp_MatZ = arma::pow(tmp, 2);

        arma::mat tmpResult = tmp_MatX + tmp_MatY + tmp_MatZ;
        tmpOutMat1.col(i) = arma::sqrt(tmpResult) - tmpOutMat2(i, 0);

        tmp = tmpOutMat1.col(i) - locMatR;
        tmpOutMat3.row(i) = XMatLib::row2col(tmp) * matD1 * tmp;

        double dexp = exp(tmpOutMat3(i, 0) * -0.5) * fen1 * locMatX3(i, 0);
        tmpOutMat4(i, 0) = dexp;

        tmpOutMat5(0, 0) += dexp;
    }

    arma::rowvec minrow = arma::min<arma::mat>(tmpOutMat3);
    if (minrow(0, 0) <= 100.0)
    {
        arma::mat out(tmp1x2, 2);
        out.zeros();
        if (tmpOutMat5(0, 0) == 0.0)
        {
            tmpMat3.col(0) = AverageMat.col(0);
            tmpMat3.col(1) = AverageMat.col(1);
            DisMat.row(0) *= 100.0;
            DisMat.row(1) *= 100.0;
            DisMat.row(2) *= 100.0;
            DisMat.row(3) *= 100.0;
        }
        else
        {
            for (int i = 0; i < tmp1x2; i++)
            {
                tmpOutMat4(i, 0) /= tmpOutMat5(0, 0);
                tmpMat3.col(0) += tmpOutMat4.row(i) * locMatX1.row(i);
                tmpMat3.col(1) += tmpOutMat4.row(i) * locMatY1.row(i);
            }
            out.insert_cols(0, locMatX1);
            out.insert_cols(1, locMatY1);
            out.shed_cols(2, 3);
            int rown = 0;
            for (int i = 0; i < tmp1x2; i++)
            {
                if (tmpOutMat4(i, 0) >= 0.01 / tmp1x2 / 400.0)
                {
                    tmpMat_d2.insert_rows(rown, out.row(i));
                    rown++;
                }
            }
            if (rown)
            {
                tmpMat_d2.shed_rows(rown, rown);
                arma::mat mat_min = arma::min<arma::mat>(tmpMat_d2);
                arma::mat mat_max = arma::max<arma::mat>(tmpMat_d2);
                DisMat.row(0) = mat_min.col(0);
                DisMat.row(1) = mat_max.col(0);
                DisMat.row(2) = mat_min.col(1);
                DisMat.row(3) = mat_max.col(1);
                tmpMat_d1.row(0) = DisMat.row(0);
                tmpMat_d1.row(1) = DisMat.row(2);
                double dtmp = DisMat(1, 0) - DisMat(0, 0);
                int itmp1 = dtmp / 10.0 + 1.0 + 0.5;
                dtmp = DisMat(3, 0) - DisMat(2, 0);
                int itmp2 = dtmp / 10.0 + 1.0 + 0.5;
                int tmp1x2 = itmp1 * itmp2;
                if (tmp1x2 <= LastFix)
                {
                    arma::mat vTmpMatX(tmp1x2, 1); vTmpMatX.zeros();
                    arma::mat vTmpMatY(tmp1x2, 1); vTmpMatY.zeros();
                    for (int i = 0; i < itmp2; i++)
                    {
                        for (int m = 0; m < itmp1; m++)
                        {
                            vTmpMatX(i * itmp1 + m, 0) = tmpMat_d1(0, 0) + m * 10;
                            vTmpMatY(i * itmp1 + m, 0) = tmpMat_d1(1, 0) + i * 10;
                        }
                    }
                    arma::mat vTmpMatX3(tmp1x2, 2);
                    vTmpMatX3.insert_cols(0, vTmpMatX);
                    vTmpMatX3.insert_cols(1, vTmpMatY);
                    vTmpMatX3.shed_cols(2, 3);

                    arma::mat vTmpMat4(tmp1x2, 1);
                    for (int i = 0; i < tmp1x2; i++)
                    {
                        arma::mat tmp = vTmpMatX3.row(i) - tmpMat3;
                        arma::mat tmp2 = (tmp)*matD2 * -0.5;
                        tmp2 *= XMatLib::row2col(tmp);
                        vTmpMat4.row(i) = arma::exp(tmp2) * fen2;
                    }

                    arma::mat OMat1(tdoa_num, tmp1x2);
                    arma::mat OMat2(tmp1x2, 1);
                    arma::mat OMat3(tmp1x2, 1);
                    arma::mat OMat4(tmp1x2, 1);

                    for (int i = 0; i < tmp1x2; i++)
                    {
                        arma::mat tmp_MatX = (locMatX.row(0) - vTmpMatX.row(i)) * (locMatX.row(0) - vTmpMatX.row(i));
                        arma::mat tmp_MatY = (locMatY.row(0) - vTmpMatY.row(i)) * (locMatY.row(0) - vTmpMatY.row(i));
                        arma::mat tmp_MatZ = (locMatZ.row(0) - fPresetHeight) * (locMatZ.row(0) - fPresetHeight);

                        arma::mat tmpResult = tmp_MatX + tmp_MatY + tmp_MatZ;
                        arma::mat mat_sqrt = arma::sqrt(tmpResult);
                        OMat2.row(i) = mat_sqrt;
                    }
                    tmpOutMat5(0, 0) = 0.0;
                    for (int i = 0; i < tmp1x2; i++)
                    {
                        arma::mat tmp = locMatX.rows(1, tdoa_num) - vTmpMatX(i, 0);
                        arma::mat tmp_MatX = arma::pow(tmp, 2);

                        tmp = locMatY.rows(1, tdoa_num) - vTmpMatY(i, 0);
                        arma::mat tmp_MatY = arma::pow(tmp, 2);

                        tmp = locMatZ.rows(1, tdoa_num) - fPresetHeight;
                        arma::mat tmp_MatZ = arma::pow(tmp, 2);

                        arma::mat tmpResult = tmp_MatX + tmp_MatY + tmp_MatZ;
                        OMat1.col(i) = arma::sqrt(tmpResult) - OMat2(i, 0);

                        tmp = OMat1.col(i) - locMatR;
                        OMat3.row(i) = XMatLib::row2col(tmp) * matD1 * tmp;

                        double dexp = exp(OMat3(i, 0) * -0.5) * fen1 * vTmpMat4(i, 0);
                        OMat4(i, 0) = dexp;

                        tmpOutMat5(0, 0) += dexp;
                    }
                    if (tmpOutMat5(0, 0) == 0.0)
                    {
                        DisMat.row(0) -= 100.0;
                        DisMat.row(1) += 100.0;
                        DisMat.row(2) -= 100.0;
                        DisMat.row(3) += 100.0;
                        tmpMat_d1.row(0) = DisMat.row(0);
                        tmpMat_d1.row(1) = DisMat.row(2);
                    }
                    else
                    {
                        tmpMat3(0, 0) = 0.0;
                        tmpMat3(0, 1) = 0.0;
                        for (int i = 0; i < tmp1x2; i++)
                        {
                            OMat4(i, 0) /= tmpOutMat5(0, 0);
                            tmpMat3.col(0) += OMat4.row(i) * vTmpMatX.row(i);
                            tmpMat3.col(1) += OMat4.row(i) * vTmpMatY.row(i);
                        }
                        int rown = 0;
                        for (int i = 0; i < tmp1x2; i++)
                        {
                            if (OMat4(i, 0) >= 0.01 / tmp1x2 / 400.0)
                            {
                                tmpMat_d3.insert_rows(rown, vTmpMatX3.row(i));
                                rown++;
                            }
                        }
                        if (rown)
                        {
                            tmpMat_d3.shed_rows(rown, rown);
                            arma::mat mat_min = arma::min<arma::mat>(tmpMat_d3);
                            arma::mat mat_max = arma::max<arma::mat>(tmpMat_d3);
                            DisMat.row(0) = mat_min.col(0);
                            DisMat.row(1) = mat_max.col(0);
                            DisMat.row(2) = mat_min.col(1);
                            DisMat.row(3) = mat_max.col(1);

                            //goto 104
                        }
                    }
                }
            }    // end if (rown)
        }
    }
    else    // if (minrow(0, 0) <= 100.0)
    {
        DisMat.row(0) -= 100.0;
        DisMat.row(1) += 100.0;
        DisMat.row(2) -= 100.0;
        DisMat.row(3) += 100.0;
        tmpMat_d1.row(0) = DisMat.row(0);
        tmpMat_d1.row(1) = DisMat.row(2);

        double dtmp = DisMat(1, 0) - DisMat(0, 0);
        int itmp1 = dtmp / 20.0 + 1.0 + 0.5;
        dtmp = DisMat(3, 0) - DisMat(2, 0);
        int itmp2 = dtmp / 20.0 + 1.0 + 0.5;
        int tmp1x2 = itmp1 * itmp2;
        if (tmp1x2 <= LastFix)
        {
            arma::mat vTmpMatX(tmp1x2, 1); vTmpMatX.zeros();
            arma::mat vTmpMatY(tmp1x2, 1); vTmpMatY.zeros();
            for (int i = 0; i < itmp2; i++)
            {
                for (int m = 0; m < itmp1; m++)
                {
                    vTmpMatX(i * itmp1 + m, 0) = tmpMat_d1(0, 0) + m * 20;
                    vTmpMatY(i * itmp1 + m, 0) = tmpMat_d1(1, 0) + i * 20;
                }
            }

            arma::mat vTmpMatX3(tmp1x2, 2);
            vTmpMatX3.insert_cols(0, vTmpMatX);
            vTmpMatX3.insert_cols(1, vTmpMatY);
            vTmpMatX3.shed_cols(2, 3);

            arma::mat locMatX3(tmp1x2, 1);
            for (int i = 0; i < tmp1x2; i++)
            {
                locMatX3(i, 0) = 1.0 / (double)tmp1x2;
            }

            arma::mat tmpOutMat1(tdoa_num, tmp1x2);
            arma::mat tmpOutMat2(tmp1x2, 1);
            arma::mat tmpOutMat3(tmp1x2, 1);
            arma::mat tmpOutMat4(tmp1x2, 1);

            for (int i = 0; i < tmp1x2; i++)
            {
                arma::mat tmp_MatX = (locMatX.row(0) - vTmpMatX.row(i)) * (locMatX.row(0) - vTmpMatX.row(i));
                arma::mat tmp_MatY = (locMatY.row(0) - vTmpMatY.row(i)) * (locMatY.row(0) - vTmpMatY.row(i));
                arma::mat tmp_MatZ = (locMatZ.row(0) - fPresetHeight) * (locMatZ.row(0) - fPresetHeight);

                arma::mat tmpResult = tmp_MatX + tmp_MatY + tmp_MatZ;
                arma::mat mat_sqrt = arma::sqrt(tmpResult);
                tmpOutMat2.row(i) = mat_sqrt;
            }
            tmpOutMat5(0, 0) = 0.0;
            for (int i = 0; i < tmp1x2; i++)
            {
                arma::mat tmp = locMatX.rows(1, tdoa_num) - vTmpMatX(i, 0);
                arma::mat tmp_MatX = arma::pow(tmp, 2);

                tmp = locMatY.rows(1, tdoa_num) - vTmpMatY(i, 0);
                arma::mat tmp_MatY = arma::pow(tmp, 2);

                tmp = locMatZ.rows(1, tdoa_num) - fPresetHeight;
                arma::mat tmp_MatZ = arma::pow(tmp, 2);

                arma::mat tmpResult = tmp_MatX + tmp_MatY + tmp_MatZ;
                tmpOutMat1.col(i) = arma::sqrt(tmpResult) - tmpOutMat2(i, 0);

                tmp = tmpOutMat1.col(i) - locMatR;
                tmpOutMat3.row(i) = XMatLib::row2col(tmp) * matD1 * tmp;

                double dexp = exp(tmpOutMat3(i, 0) * -0.5) * fen1 * locMatX3(i, 0);
                tmpOutMat4(i, 0) = dexp;

                tmpOutMat5(0, 0) += dexp;
            }
            if (tmpOutMat5(0, 0) == 0.0)
            {
                DisMat.row(0) -= 100.0;
                DisMat.row(1) += 100.0;
                DisMat.row(2) -= 100.0;
                DisMat.row(3) += 100.0;
                tmpMat_d1.row(0) = DisMat.row(0);
                tmpMat_d1.row(1) = DisMat.row(2);
            }
            else
            {
                tmpMat3(0, 0) = 0.0;
                tmpMat3(0, 1) = 0.0;
                for (int i = 0; i < tmp1x2; i++)
                {
                    tmpOutMat4(i, 0) /= tmpOutMat5(0, 0);
                    tmpMat3.col(0) += tmpOutMat4.row(i) * vTmpMatX.row(i);
                    tmpMat3.col(1) += tmpOutMat4.row(i) * vTmpMatY.row(i);
                }
                int rown = 0;
                for (int i = 0; i < tmp1x2; i++)
                {
                    if (tmpOutMat4(i, 0) >= 0.01 / tmp1x2 / 400.0)
                    {
                        tmpMat_d3.insert_rows(rown, vTmpMatX3.row(i));
                        rown++;
                    }
                }
                if (rown)
                {
                    tmpMat_d3.shed_rows(rown, rown);
                    arma::mat mat_min = arma::min<arma::mat>(tmpMat_d3);
                    arma::mat mat_max = arma::max<arma::mat>(tmpMat_d3);
                    DisMat.row(0) = mat_min.col(0);
                    DisMat.row(1) = mat_max.col(0);
                    DisMat.row(2) = mat_min.col(1);
                    DisMat.row(3) = mat_max.col(1);
                }
            }
        }
    }

    //label_104
    arma::mat tmp = locMatX - tmpMat3(0, 0);
    arma::mat OutX = arma::pow(tmp, 2);

    tmp = locMatY - tmpMat3(0, 1);
    arma::mat OutY = arma::pow(tmp, 2);

    tmp = locMatZ - fPresetHeight;
    arma::mat OutZ = arma::pow(tmp, 2);

    arma::mat tmpResult = OutX + OutY + OutZ;
    arma::mat mat_sqrt = arma::sqrt(tmpResult);
    tmp = mat_sqrt.rows(1, tdoa_num) - mat_sqrt(0, 0);
    tmp = tmp - locMatR;
    tmp = (XMatLib::row2col(tmp) * tmp) / tdoa_num;
    if (tmp(0, 0) > 10000.0)
    {
        tmpMat3.col(0) = AverageMat.col(0);
        tmpMat3.col(1) = AverageMat.col(1);
    }
    InsertToLoc12(pTagPos->tagId,
        pTagPos->b18,
        tmpMat3(0, 0),
        tmpMat3(0, 1),
        DisMat(0, 0),
        DisMat(1, 0),
        DisMat(2, 0),
        DisMat(3, 0)
    );

    ret = 1;

    return ret;
}
/***************************************************************************************************************
* NOTE      |
* ParamIn   |
* ParamOut  |
* Return    |
***************************************************************************************************************/
int LocateM3(ALG_LOCADATA* pLocData)
{
    int ret = 0;
    std::vector<double> vec_PosX;
    std::vector<double> vec_PosY;
    std::vector<double> vec_PosZ;
    std::vector<double> vec_R;

    GetPosFromAnchor(vec_PosX, pLocData, 'x');
    GetPosFromAnchor(vec_PosY, pLocData, 'y');
    GetPosFromAnchor(vec_PosZ, pLocData, 'z');
    GetPosFromAnchor(vec_R, pLocData, 'r');

    double min_PosX = 0.0, max_PosX = 0.0;
    if (vec_PosX.size() > 0)
    {
        min_PosX = *min_element(vec_PosX.begin(), vec_PosX.end());
        max_PosX = *max_element(vec_PosX.begin(), vec_PosX.end());
    }
    double min_PosY = 0.0, max_PosY = 0.0;
    if (vec_PosY.size() > 0)
    {
        min_PosY = *min_element(vec_PosY.begin(), vec_PosY.end());
        max_PosY = *max_element(vec_PosY.begin(), vec_PosY.end());
    }

    MAP_LOC12_NODE::iterator it = g_Loc12.find(pLocData->tagInfo.TagId);
    Loc12_Node_Item* pTagPos = nullptr;
    if (it == g_Loc12.end())
    {
        InsertToLoc12(pLocData->tagInfo.TagId,
            pLocData->tagInfo.b18,
            (max_PosX + min_PosX) / 2,
            (max_PosY + min_PosY) / 2,
            min_PosX,
            max_PosX,
            min_PosY,
            max_PosY
        );

        pTagPos = &g_Loc12[pLocData->tagInfo.TagId];
    }
    else
        pTagPos = &it->second;

    double factor = GetFactor(pLocData->tagInfo.b19);
    int calcret = CalcM3(vec_PosX, vec_PosY, vec_PosZ, vec_R,
        pTagPos, pLocData->tagInfo.b18, factor, pLocData->tagInfo.fPresetHeight);;

    if (!calcret)
    {
        if (vec_PosX.size() > 0)
        {
            min_PosX = *min_element(vec_PosX.begin(), vec_PosX.end());
            max_PosX = *max_element(vec_PosX.begin(), vec_PosX.end());
        }
        if (vec_PosY.size() > 0)
        {
            min_PosY = *min_element(vec_PosY.begin(), vec_PosY.end());
            max_PosY = *max_element(vec_PosY.begin(), vec_PosY.end());
        }

        InsertToLoc12(pLocData->tagInfo.TagId,
            pLocData->tagInfo.b18,
            (max_PosX + min_PosX) / 2,
            (max_PosY + min_PosY) / 2,
            min_PosX,
            max_PosX,
            min_PosY,
            max_PosY
        );

        pTagPos = &g_Loc12[pLocData->tagInfo.TagId];
        calcret = CalcM3(vec_PosX, vec_PosY, vec_PosZ, vec_R, pTagPos, pLocData->tagInfo.b18, factor, pLocData->tagInfo.fPresetHeight);
        if (!calcret)
        {
            pLocData->tagInfo.err_94 = 93;
            ret = 0;
        }
    }
    else
    {
        pLocData->tagInfo.bMat_CalOk = 1;
        pLocData->tagInfo.Mat_PosX = pTagPos->average_X;
        pLocData->tagInfo.Mat_PosY = pTagPos->average_Y;
        pLocData->tagInfo.Mat_PosZ = pLocData->tagInfo.fPresetHeight;
        ret = 1;
    }

    return ret;
}
/***************************************************************************************************************
* NOTE      |
* ParamIn   |
* ParamOut  |
* Return    |
***************************************************************************************************************/
void Loc1(ALG_LOCADATA* pLocData)
{
    CalcTdoa(pLocData);
    LocateM3(pLocData);
}
/***************************************************************************************************************
* NOTE      |
* ParamIn   |
* ParamOut  |
* Return    |
***************************************************************************************************************/
void get_pos(uint64_t ts_1, uint64_t ts_2, uint64_t ts_3, uint64_t ts_4)
{
    alg_data.anchor_data[0].timestamp2 =alg_data.anchor_data[0].timestamp = ts_1;
    alg_data.anchor_data[1].timestamp2 = alg_data.anchor_data[1].timestamp = ts_2;
    alg_data.anchor_data[2].timestamp2 = alg_data.anchor_data[2].timestamp = ts_3;
    alg_data.anchor_data[3].timestamp2 = alg_data.anchor_data[3].timestamp = ts_4;

    Loc1(&alg_data);
}

void  get_pos_temp(uint64_t ts_1, uint64_t ts_2, uint64_t ts_3, uint64_t ts_4)
{
    uint32_t Xmin, Xmax, Ymin, Ymax,Zmin,Zmax;
    double TEMP1 = 10000000, TEMP2 = 10000000, TEMP3 = 10000000;
    double K1, K2, K3, K4, K5, K6;

    double d12 = (double)((int64_t)ts_1 - (int64_t)ts_2) / 2.13;
    double d23 = (double)((int64_t)ts_2 - (int64_t)ts_3)/2.13;
    double d34 = (double)((int64_t)ts_3 - (int64_t)ts_4) / 2.13;
    double d41 = (double)((int64_t)ts_4 - (int64_t)ts_1) / 2.13;
    double d13 = (double)((int64_t)ts_1 - (int64_t)ts_3) / 2.13;
    double d24 = (double)((int64_t)ts_2 - (int64_t)ts_4) / 2.13;
    
    Xmin = (alg_data.tagInfo.Mat_PosX - 100 > MapXmin ? (alg_data.tagInfo.Mat_PosX - 100) : MapXmin);
    Xmax = (alg_data.tagInfo.Mat_PosX + 100 > MapXmax ? MapXmax: (alg_data.tagInfo.Mat_PosX + 100) );

    Ymin = (alg_data.tagInfo.Mat_PosX - 100 > MapYmin ? (alg_data.tagInfo.Mat_PosY - 100) : MapYmin);
    Ymax = (alg_data.tagInfo.Mat_PosX + 100 > MapYmax ? MapXmax : (alg_data.tagInfo.Mat_PosY + 100));

    Zmin = 40; Zmax = 150;
#if 1
    for (uint32_t i = Zmin;i< Zmax;i++)
    {
        for (uint32_t j = Xmin; j < Xmax; j++)
        {
            for (uint32_t w = Ymin; w < Ymax; w++)
            {
                K1 = sqrt(pow(BS[0][0] - j, 2) + pow(BS[0][1] - w, 2) + pow(BS[0][2] - i, 2)) - sqrt(pow(j - BS[1][0], 2) + pow(w - BS[1][1], 2) + pow(i - BS[1][2], 2));
                K2 = sqrt(pow(BS[1][0] - j, 2) + pow(BS[1][1] - w, 2) + pow(BS[1][2] - i, 2)) - sqrt(pow(j - BS[2][0], 2) + pow(w - BS[2][1], 2) + pow(i - BS[2][2], 2));
                K3 = sqrt(pow(BS[2][0] - j, 2) + pow(BS[2][1] - w, 2) + pow(BS[2][2] - i, 2)) - sqrt(pow(j - BS[3][0], 2) + pow(w - BS[3][1], 2) + pow(i - BS[3][2], 2));
                K4 = sqrt(pow(BS[3][0] - j, 2) + pow(BS[3][1] - w, 2) + pow(BS[3][2] - i, 2)) - sqrt(pow(j - BS[0][0], 2) + pow(w - BS[0][1], 2) + pow(i - BS[0][2], 2));
                K5 = sqrt(pow(BS[0][0] - j, 2) + pow(BS[0][1] - w, 2) + pow(BS[0][2] - i, 2)) - sqrt(pow(j - BS[2][0], 2) + pow(w - BS[2][1], 2) + pow(i - BS[2][2], 2));
                K6 = sqrt(pow(BS[1][0] - j, 2) + pow(BS[1][1] - w, 2) + pow(BS[1][2] - i, 2)) - sqrt(pow(j - BS[3][0], 2) + pow(w - BS[3][1], 2) + pow(i - BS[3][2], 2));

                if (TEMP1 > pow(d12- K1,2)+ pow(d23 - K2, 2) + pow(d34 - K3, 2) + pow(d41 - K4, 2))
                {
                    TEMP1 = pow(d12 - K1, 2) + pow(d23 - K2, 2) + pow(d34 - K3, 2) + pow(d41 - K4, 2);
                    a[0][0] = j;
                    a[0][1] = w;
                    a[0][2] = i;
                    
                }

                if (TEMP2 > pow(d13-K5,2)+ pow(d24 - K6, 2))
                {
                    TEMP2 = pow(d13 - K5, 2) + pow(d24 - K6, 2);
                    a[1][0] = j;
                    a[1][1] = w;
                    a[1][2] = i;
                
                }

                if (TEMP3 > pow(d13 - K5, 2) + pow(d41 - K4, 2) + pow(d12 - K1, 2))
                {
                    TEMP3 = pow(d13 - K5, 2) + pow(d41 - K4, 2) + pow(d12 - K1, 2);
                    a[2][0] = j;
                    a[2][1] = w;
                    a[2][2] = i;


                }
                w += 1;
            }
            j += 1;
        }
        i += 3;
    }

    printf("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\r\n", a[0][0], a[0][1], a[0][2], a[1][0], a[1][1], a[1][2], a[2][0], a[2][1], a[2][2]);

    a[3][0] = a[0][0] * 0.25 + a[1][0] * 0.25 + a[2][0] * 0.3 + alg_data.tagInfo.Mat_PosX * 0.2;
    a[3][1] = a[0][1] * 0.25 + a[1][1] * 0.25 + a[2][1] * 0.3 + alg_data.tagInfo.Mat_PosY * 0.2;
    a[3][2] = a[1][2];
#endif
}

 

试探你的温柔耶
关注
  • 0
    点赞
  • 踩
  • 0
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
专栏目录
SQLAllocHandle — 分配句柄
fulminate的专栏
09-29 853
用途 SQLAllocHandle() 分配任何类型的句柄。 语法 SQLRETURN SQLAllocHandle (SQLSMALLINT htype, SQLINTEGER ihandle, SQLINTEGER *handle); 函数自变量 表 10
diag()函数功能
热门推荐
porly的专栏
08-16 7万+
关于matlab中的diag函数（矩阵对角元素的提取和创建对角阵） diag函数功能：矩阵对角元素的提取和创建对角阵 设以下X为方阵，v为向量 1、X = diag(v,k)当v是一个含有n个元素的向量时，返回一个n+abs(k)阶方阵X，向量v在矩阵X中的第k个对角线上，k=0表示主对角线，k>0表示在主对角线上方，k<0表示在主对角线下方。例1： v=[1 2 3]; diag
OpenCV for Matlab Users (2) - cv::Mat 类的属性和方法
翻肚鱼儿的博客
03-09 508
目前成员变量只有 5 个：rows，cols，dims，data，step，还有一些比如像 channels，depth，elemSize，empty，isContinuous，size，total，type 貌似是成员变量，其实是没有参数的成员方法。 1. 成员变量 rows 成员变量，返回矩阵的行数 int imgRows = img.rows; std::cout<<"imgRows = "<<imgRows<<std::end.
RcppArmadillo简明手册
iamsuperman2的博客
06-12 1万+
RcppArmadillo
2020-05-08
weixin_41208789的博客
05-08 791
见字如面: 因为学习及进取需要，准备从RNA-seq和ChIP-seq分析开启生信之路。在查阅资料的过程中，从简书，CSDN，及知乎上学到了很多，特注册账号，已备日后留文。生信之路，必将艰难重重，bug不断。定将不断摸索，掌握技能，期以日后自己能分享一些有价值的东西予人，不甚乐哉！特留一段文字，给予自己动力！ lierkui 2020-05-07晚于北京 ...
2020-04-08
qq_41637959的博客
04-08 799
LoadRunner基础使用教程
2020-05-31
GJJailang的博客
05-31 924
现在后端不做云计算，大数据，人工智能，底层程序员是不是只剩增删改查了。感觉自己混了两年混成运维了。
TianchiNLP 2020-08-05 08:44:30-数据集
03-25
【标题】：“TianchiNLP 2020-08-05 08:44:30-数据集”是一个与自然语言处理（NLP）相关的数据集，发布于2020年8月5日，可能是阿里云天池平台上的一个竞赛或者研究项目的一部分。这个数据集可能包含了用于训练和测试...
2020-05-05-12-49-45.mp4
05-05
简单的Fluent热流固耦合案例分析，模型参考意义不大，主要讲述了热流固耦合的思想，对快速入手软件有很大的帮助
2020-05-08测验9单项选择题.pdf
最新发布
06-11
2020-05-08测验9单项选择题 2020-05-08测验9单项选择题 单项选择题 单项选择题 点击选项选择正确答案，每题有且仅有⼀个正确答案 1 以下选项不是Python数据可视化⽅向第三⽅库的是： A Seaborn B Mayavi C ...
Draft 2020-05-08 12:02:58-数据集
03-14
在这个特定的案例中，我们有一个名为"Draft 2020-05-08 12:02:58 - 数据集"的压缩包，其中包含了"job_detail_dataset.csv"这个文件。这显然是一个关于招聘的数据集，很可能包含了各种与招聘过程相关的信息。 CSV...
art-K2T-2020-05-08.bin
05-09
K2T 108的art数据 是官改固件 是官改固件是官改固件是官改固件 我的刷咯没5G 不知道要怎么版好咯
[论文笔记] Session-aware Linear Item-Item Models for Session-based Recommendation
THOOOR的博客
08-19 461
原文地址 Session-aware Linear Item-Item Models for Session-based Recommendation (arxiv.org) https://github.com/jin530/SLIST 基于会话的推荐旨在根据会话中先前消费的物品的序列预测下一个物品，例如电子商务或多媒体流服务。 具体来说，会话数据表现出一些独特的特征，即会话一致性和会话内物品的顺序依赖性、重复的物品消费和会话时效性。 本文提出了简单而有效的线性模型来考虑会话的整体方面。具体来说： .
VFP检测SQL Server的五个实例代码
weixin_41999297的博客
02-11 391
** 需要指出的是，无论下面哪种方式的代码，都需要打开本机的网络共享，否则找不到SQL服务器 ** 例一 *************************************************** * Use MyFll.Fll(下载MyFll) Set Library To MyFll lcLocalName = Left(Sys(0),At("#",Sys(0))-2) lcLoc...
c语言ODBC连接数据库
水神
09-27 7120
在vc++ 6.0里，用c++通过ODBC API,使用函数SQLConnect连接数据库。 #include    #include    #include "sqlext.h"   using namespace std; /*用ODBC API访问数据库:连接数据库*/ int main(){     SQLHENV henv;  //环境句柄        SQ
QT定位地图制作
Gavin的博客
08-05 2353
一，qt绘画图形 1.创建绘图事件函数。 <1>. paintEvent(QPaintEvent*){} <2>. <2>. <3>.
QT WebSocket的实现
Gavin的博客
08-08 1945
一 ，new一个QWebsocke()服务； m_timer = new QTimer(); dataRecvWS = Q_NULLPTR; if(!dataRecvWS) { dataRecvWS = new QWebSocket(); } 2，通过按键控制连接与断开 <1>,信号与曹函数绑定，按键触发中断 connect(dataRecvWS,SIGNAL(connected()), this,SLOT(on..
QT 数据类型转换笔记
Gavin的博客
10-26 1838
一，转换成QByteArry 1、QString转QByteArray QByteArray byte; QString string; byte = string.toLatin1(); 2、char *转QByteArray （不能是char ch[n]） char *ch; QByteArray byte; byte = QByteArray(ch); 3、char ch[n]转QByteArray (n为uint_t 类型) uint8_t ch[10]={1,2,3,4,5,6,7,8
A集合： [{begintime:2020-01-01 00:04:00,endtime:2020-01-01 00:06:00}, {begintime:2020-01-01 00:01:05,endtime:2020-01-01 00:03:15}, {begintime:2020-01-01 00:09:00,endtime:2020-01-01 00:10:00}, {begintime:2020-01-01 00:07:35,endtime:2020-01-01 00:08:40}, {begintime:2020-01-01 00:12:00,endtime:2020-01-01 00:17:00}, {begintime:2020-01-01 00:12:05,endtime:2020-01-01 00:13:10}, {begintime:2020-01-01 00:19:00,endtime:2020-01-01 00:21:00}, {begintime:2020-01-01 00:15:20,endtime:2020-01-01 00:20:45}]  期望得到的输出结果： [{begintime:2020-01-01 00:01:05,endtime:2020-01-01 00:03:15}, {begintime:2020-01-01 00:04:00,endtime:2020-01-01 00:06:00}, {begintime:2020-01-01 00:07:35,endtime:2020-01-01 00:08:40}, {begintime:2020-01-01 00:09:00,endtime:2020-01-01 00:10:00}, {begintime:2020-01-01 00:12:00,endtime:2020-01-01 00:21:00}]
04-23
[{begintime:2020-01-01 00:01:05,endtime:2020-01-01 00:03:15}, {begintime:2020-01-01 00:04:00,endtime:2020-01-01 00:06:00}, {begintime:2020-01-01 00:07:35,endtime:2020-01-01 00:08:40}, {begintime:2020-...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

试探你的温柔耶

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值