轨迹点整合与保存

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

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 TRACESAVING

struct 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 TRACESAVING

extern 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)));
}