轨迹点整合与保存

来源：互联网发布：手机绘图软件artrage 编辑：程序博客网时间：2024/06/16 09:40

单独跑一个线程，执行写入任务

TraceSaving *mTraceSaving;mTraceSaving = new TraceSaving;    std::thread *tracesaveThread = new std::thread(&TraceSaving::saveTracePoints, mTraceSaving);

TraceSaving.h

#pragma once#include <vector>#include <iostream>#include <Windows.h>#include "HalconCpp.h"#include <time.h>#include <stdio.h>#include <stdlib.h>#include <strsafe.h>#include "trackMode.h"#define TRACESAVINGstruct TracePoints{    double point[3];    double t;};class TraceSaving{public:    TraceSaving();    ~TraceSaving();public:    void getCaptruePointInitial(const double measure[12]);    void getCapturePointInTrack(const double measure[4]);    void getPredictPoint(double *pEndPosBuf, unsigned int uiEndPosBufNum);    double get_current_time(void);    void saveTracePoints();private:    bool binarySearch(std::vector<TracePoints> &vTracePoints, const double measure[12]);    int binarySearch(double t, std::vector<TracePoints> &vTracePoints, int low, int high);    double calDistance(int Index, std::vector<TracePoints> &vTracePoints, const double measure[4]);public:    double dEndPointTime;    bool bWriteIn;    std::vector<TracePoints> vTracePoints;    std::vector<TracePoints> vPredictPoints;    FILE *fp = NULL;};

TraceSaving.cpp

#include "TestStatistics\TraceSaving.h"#include <iostream>#include <iomanip>#define TRACESAVINGextern HalconCpp::HTuple curTime;TraceSaving::TraceSaving(){        vTracePoints.clear();        dEndPointTime = -1;        bWriteIn = false;}TraceSaving::~TraceSaving(){}void TraceSaving::getCaptruePointInitial(const double measure[12]){    //读取当前时间    //HalconCpp::CountSeconds(&currentT);    TracePoints sTracePointTmp;    if (!bWriteIn)    {        if (measure[3] < dEndPointTime)        {            if (!vPredictPoints.empty())            {                if (binarySearch(vPredictPoints, measure))                    std::cout << "[Message]: connect two trace points SUCCESS!" << std::endl;            }               }        else if (dEndPointTime = -1)        {            for (int i = 0; i < 3; ++i){                sTracePointTmp.point[0] = measure[4 * i + 0];                sTracePointTmp.point[1] = measure[4 * i + 1];                sTracePointTmp.point[2] = measure[4 * i + 2];                sTracePointTmp.t = measure[4 * i + 3];                vTracePoints.push_back(sTracePointTmp);            }        }        else            bWriteIn = true;    }    else        std::cout << "vTracePoints has been written in, ignore this trace!" << std::endl;}void TraceSaving::getCapturePointInTrack(const double measure[4]){    TracePoints sTracePointTmp;    if (!bWriteIn)    {        if (!vTracePoints.empty())        {            sTracePointTmp.point[0] = measure[0];            sTracePointTmp.point[1] = measure[1];            sTracePointTmp.point[2] = measure[2];            sTracePointTmp.t = measure[3];            vTracePoints.push_back(sTracePointTmp);        }    }}void TraceSaving::getPredictPoint(double *pEndPosBuf, unsigned int uiEndPosBufNum){    TracePoints sTracePointTmp;    vPredictPoints.clear();    for (int i = 0; i < uiEndPosBufNum; i++)    {        sTracePointTmp.point[0] = pEndPosBuf[i * 4 + 0];        sTracePointTmp.point[1] = pEndPosBuf[i * 4 + 1];        sTracePointTmp.point[2] = pEndPosBuf[i * 4 + 2];        sTracePointTmp.t = pEndPosBuf[i * 4 + 3] + curTime[0].D();        vPredictPoints.push_back(sTracePointTmp);    }    dEndPointTime = vPredictPoints.back().t;}double TraceSaving::get_current_time(void){    HTuple t;    CountSeconds(&t);    return t.ToDArr()[0];}void TraceSaving::saveTracePoints(){    while (1)    {        if (get_current_time() > dEndPointTime && dEndPointTime > 0)        {            bWriteIn = true;        }        if (bWriteIn)        {            fp = fopen("src/TestStatistics/TracePoints.dat", "a+");            if (fp != NULL)            {                char sTrace[100];                sprintf_s(sTrace, 100, "New Trace:                                                %d\n", vTracePoints.size());                fwrite(sTrace, strnlen_s(sTrace, 100), 1, fp);                for (int i = 0; i < vTracePoints.size(); i++)                {                    //fwrite(buffer, strnlen_s(buffer, 100), 1, fp);                    //sprintf_s(buffer, "%lf\n", Distance);                    char str[100];                    sprintf_s(str, "     %8lf     %8lf     %8lf     %8lf\n", vTracePoints[i].point[0], vTracePoints[i].point[1], vTracePoints[i].point[2], vTracePoints[i].t);                    fwrite(str, strnlen_s(str, 100), 1, fp);                    //std::cout << std::setw(10) << vTracePoints[i].point[0] << std::setw(10) << vTracePoints[i].point[1] << std::setw(10) << vTracePoints[i].point[2] << std::setw(10) << vTracePoints[i].t << std::endl;                }                vTracePoints.clear();                bWriteIn = false;                dEndPointTime = -1;                fclose(fp);            }            else            {                std::cerr << "Open file TracePoints.dat failed!" << std::endl;            }        }        else            Sleep(10);    }    return;}bool TraceSaving::binarySearch(std::vector<TracePoints> &vTracePoints, const double measure[12]){    int iIndex;    TracePoints sTracePointTmp;    if ((iIndex = binarySearch(measure[3], vTracePoints, 0, vTracePoints.size() - 1)) != -1)    {        if (calDistance(iIndex, vTracePoints, measure) < 0.4)        {            for (int i = 0; i < 3; i++)            {                sTracePointTmp.point[0] = measure[4 * i + 0];                sTracePointTmp.point[1] = measure[4 * i + 1];                sTracePointTmp.point[2] = measure[4 * i + 2];                sTracePointTmp.t = measure[4 * i + 3];                vTracePoints.push_back(sTracePointTmp);            }            return true;        }    }    //捕捉点与轨迹匹配失败，保存之前所有轨迹点    bWriteIn = true;    return false;}int TraceSaving::binarySearch(double t, std::vector<TracePoints> &vTracePoints, int low, int high){    if (low <= high)    {        int mid = (low + high) / 2;        std::cout << "mid is: " << mid << "vTracePointsCnt is: " << vTracePoints.size() << std::endl;        if (fabs(t - vTracePoints[mid].t) <= 0.005)        {            return mid;        }        else if (t < vTracePoints[mid].t)        {            return binarySearch(t, vTracePoints, low, mid);        }        else            return binarySearch(t, vTracePoints, mid, high);    }    return -1;}double TraceSaving::calDistance(int Index, std::vector<TracePoints> &vTracePoints, const double measure[4]){    return std::sqrtf((std::pow((measure[0] - vTracePoints[Index].point[0]), 2)        + std::pow((measure[1] - vTracePoints[Index].point[2]), 2)        + std::pow((measure[2] - vTracePoints[Index].point[2]), 2)));}

0 0