modularization/src/fusion/lidar_radar_fusion_cnn/lidarbuffer.cpp

#include "lidarbuffer.h"

#include <iostream>
#include "math/gnss_coordinate_convert.h"

lidarbuffer::lidarbuffer(int64_t nLookTime,double fProb_Thresh,double fLidarOffX, double fLidarOffY, double fDisThresh)
{
    mnLookTime = nLookTime;
    mfProb_Thresh = fProb_Thresh;
    mfLidarOffX = fLidarOffX;
    mfLidarOffY = fLidarOffY;
    mfDisThresh = fDisThresh;
}

void lidarbuffer::AddGPS(iv::gps::gpsimu & xgpsimu)
{
    const int nMaxGPSFre = 100;
    if(mvectorgps_rec.size() > static_cast<unsigned int>( (mnLookTime * nMaxGPSFre*2/1000) ) )
    {
        std::cout<<" Warning. GPS Buffer Warning."<<std::endl;
        mmutex.lock();
        mvectorgps_rec.clear();;
        mmutex.unlock();
    }

    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;

    mmutex.lock();
    while(mvectorgps_rec.size()>0)
    {
        if(abs(nnow - mvectorgps_rec[0].nTime)>static_cast<unsigned int>( mnLookTime))
        {
            mvectorgps_rec.erase(mvectorgps_rec.begin());
        }
        else
        {
            break;
        }
    }
    mmutex.unlock();

    mmutex.lock();
    iv::gps_rec xrec;
    xrec.nTime = xgpsimu.msgtime();
    xrec.xgpsimu.CopyFrom(xgpsimu);
    mvectorgps_rec.push_back(xrec);
    mmutex.unlock();
}

void lidarbuffer::AddLidarObj(iv::lidar::objectarray & xobjarray)
{
    const int nMaxLidarFre = 100;
    if(mvectorgps_rec.size() > static_cast<unsigned int>( (mnLookTime * nMaxLidarFre*2/1000) ) )
    {
        std::cout<<" Warning. Lidar Buffer Warning."<<std::endl;
        mmutex.lock();
        mvectorlidarobj_rec.clear();;
        mmutex.unlock();
    }

    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;

    mmutex.lock();
    while(mvectorlidarobj_rec.size()>0)
    {
        if(abs(nnow - mvectorlidarobj_rec[0].nTime)>static_cast<unsigned int>( mnLookTime))
        {
            mvectorlidarobj_rec.erase(mvectorlidarobj_rec.begin());
        }
        else
        {
            break;
        }
    }
    mmutex.unlock();

    mmutex.lock();
    iv::lidarobj_rec xrec;
    xrec.nTime = xobjarray.timestamp(); //to ms

    xrec.xobjarray.CopyFrom(xobjarray);
    ChangePos(&xrec);

    mvectorlidarobj_rec.push_back(xrec);
    mmutex.unlock();
}

void lidarbuffer::ChangePos(iv::lidarobj_rec * xrec)
{
    if(mvectorgps_rec.size() == 0)
    {
        std::cout<<" warning no gps data. "<<std::endl;
        return;
    }

    unsigned int nnearindex = 0;
    int64_t ntimediff = mnLookTime;

//    xrec->nTime = std::chrono::system_clock::now().time_since_epoch().count()/1000000 - 100;

    unsigned int nsize = static_cast<unsigned int>( mvectorgps_rec.size());
    unsigned int i;
    for(i=1;i<nsize;i++)
    {
        if(abs(xrec->nTime - mvectorgps_rec[i].nTime)<abs(ntimediff))
        {
            ntimediff = xrec->nTime - mvectorgps_rec[i].nTime;
            nnearindex = i;
        }
    }

    if(abs(ntimediff) > 100)
    {
        std::cout<<"warning. lidar gps time diff more than 100ms"<<std::endl;
    }

    iv::gps::gpsimu xgpsimu;
    xgpsimu.CopyFrom(mvectorgps_rec[nnearindex].xgpsimu);

    double hdg = (90 - xgpsimu.heading())*M_PI/180.0;

    double fsr = hdg - M_PI/2.0;
    double x_sensor = mfLidarOffX * cos(fsr) - mfLidarOffY * sin(fsr);
    double y_sensor = mfLidarOffX * sin(fsr) + mfLidarOffY * cos(fsr);

    xrec->sensor_hdg = hdg;
    xrec->sensor_x = x_sensor;
    xrec->sensor_y = y_sensor;

    double x_gps,y_gps;
    GaussProjCal(xgpsimu.lon(),xgpsimu.lat(),&x_gps,&y_gps);

    nsize = xrec->xobjarray.obj_size();
    for(i=0;i<nsize;i++)
    {
        iv::lidar::lidarobject * pobj = xrec->xobjarray.mutable_obj(i);
        double x_raw = pobj->position().x();
        double y_raw = pobj->position().y();

        double x_abs = x_gps + x_sensor + x_raw * cos(fsr) - y_raw * sin(fsr);
        double y_abs = y_gps + y_sensor + x_raw * sin(fsr) + y_raw * cos(fsr);

        pobj->mutable_position()->set_x(x_abs);
        pobj->mutable_position()->set_y(y_abs);


    }

    xrec->sensor_x = xrec->sensor_x + x_gps;
    xrec->sensor_y = xrec->sensor_y + y_gps;
}

int lidarbuffer::MergeLast(iv::lidar::objectarray & xselarray)
{
    int nsel = 0;
    if(mvectorlidarobj_rec.size() == 0)return 0;
    mmutex.lock();

    int nsize = mvectorlidarobj_rec.size();
    int i;
    std::vector<std::vector<iv::stats_count>> xvectorcount_series;
    for(i=0;i<nsize;i++)
    {
        unsigned int j;
        unsigned int nobjsize = mvectorlidarobj_rec[i].xobjarray.obj_size();
        std::vector<iv::stats_count> xvectorcount;
        for(j=0;j<nobjsize;j++)
        {
            iv::stats_count sc;
            sc.ncount = 1;
            sc.nIndex_vector = i;
            sc.nIndex_obj = j;
            sc.nSameRef = -1;
            xvectorcount.push_back(sc);
        }
        if(xvectorcount.size()>0) xvectorcount_series.push_back(xvectorcount);
    }

    std::vector<std::vector<iv::stats_count>> xvectorcount_same;


    while(xvectorcount_series.size()>0)
    {
        if(xvectorcount_series[0].size() == 0)
        {
            xvectorcount_series.erase(xvectorcount_series.begin());
            continue;
        }
        iv::stats_count scref = xvectorcount_series[0].at(0);
        xvectorcount_series[0].erase(xvectorcount_series[0].begin());
        std::vector<iv::stats_count> xvectorsa;
        xvectorsa.push_back(scref);


        iv::lidar::lidarobject * pobj = mvectorlidarobj_rec[scref.nIndex_vector].xobjarray.mutable_obj(scref.nIndex_obj);

        for(i=1;i<static_cast<int>( xvectorcount_series.size());i++)
        {
            double fdismin = 1000.0;
            int nindexmin = -1;
            int j;
            for(j=0;j<static_cast<int>( xvectorcount_series[i].size());j++)
            {
               iv::lidar::lidarobject * pobj2 = mvectorlidarobj_rec[xvectorcount_series[i].at(j).nIndex_vector].xobjarray.mutable_obj(xvectorcount_series[i].at(j).nIndex_obj);
               double fdis =sqrt(pow(pobj->mutable_position()->x() -  pobj2->mutable_position()->x(),2)
                                 +pow(pobj->mutable_position()->y() - pobj2->mutable_position()->y(),2));
               if(fdis<fdismin)
               {
                   fdismin = fdis;
                   nindexmin = j;
               }
            }
            if(nindexmin>=0)
            {
                if(fdismin<mfDisThresh)
                {
                    xvectorsa.push_back(xvectorcount_series[i].at(nindexmin));
                    xvectorcount_series[i].erase(xvectorcount_series[i].begin() + nindexmin);
                }
            }

        }
        xvectorcount_same.push_back(xvectorsa);

    }

    //Select Object
    for(i=0;i<static_cast<int>( xvectorcount_same.size());i++)
    {
        double fprob = ((double)xvectorcount_same[i].size())/((double)nsize);
        if(fprob >= mfProb_Thresh)
        {
            iv::lidar::lidarobject * pobj = xselarray.add_obj();
            iv::stats_count sc = xvectorcount_same[i].at(xvectorcount_same[i].size() -1);
            pobj->CopyFrom(*mvectorlidarobj_rec[sc.nIndex_vector].xobjarray.mutable_obj(sc.nIndex_obj));
            nsel++;
        }
    }

    mmutex.unlock();
    return nsel;
}

int lidarbuffer::FilterLidarObj(iv::lidar::objectarray & xobjarray)
{
    int nAdd = 0;
    iv::lidar::objectarray xlast;
    MergeLast(xlast);

    iv::lidarobj_rec xrec;
    xrec.nTime = xobjarray.timestamp(); //to ms
    xrec.xobjarray.CopyFrom(xobjarray);
    mmutex.lock();
    ChangePos(&xrec);
    mmutex.unlock();

    iv::lidar::objectarray xobjarraylast;

    int i;
    for(i=0;i<xlast.obj_size();i++)
    {
        iv::lidar::lidarobject * pobjlast = xlast.mutable_obj(i);
        double fdismin = 1000.0;
        int nindexmin = -1;
        int j;
        for(j=0;j<xrec.xobjarray.obj_size();j++)
        {
            iv::lidar::lidarobject * pobjnow = xrec.xobjarray.mutable_obj(j);
            double fdis = sqrt(pow(pobjlast->mutable_position()->x() - pobjnow->mutable_position()->x(),2)
                               +pow(pobjlast->mutable_position()->y() - pobjnow->mutable_position()->y(),2));
            if(fdis<fdismin)
            {
                fdismin = fdis;
                nindexmin = j;
            }
        }
        if(fdismin< mfDisThresh)
        {
            //same
        }
        else
        {
            iv::lidar::lidarobject * pobj = xobjarraylast.add_obj();
            pobj->CopyFrom(*pobjlast);
            nAdd++;
        }
    }

    if(nAdd>0)
    {
        int i;
        for(i=0;i<xobjarraylast.obj_size();i++)
        {
            iv::lidar::lidarobject * pobj = xobjarraylast.mutable_obj(i);
            double x_abs = pobj->mutable_position()->x();
            double y_abs = pobj->mutable_position()->y();
            double x_rel,y_rel;
            x_rel = x_abs - xrec.sensor_x;
            y_rel = y_abs - xrec.sensor_y;

            double fsr = 0 + M_PI/2.0 - xrec.sensor_hdg;

            double x_raw =   x_rel * cos(fsr) - y_rel * sin(fsr);
            double y_raw =   x_rel * sin(fsr) + y_rel * cos(fsr);
            pobj->mutable_position()->set_x(x_raw);
            pobj->mutable_position()->set_y(y_raw);
            std::cout<<" add: x:"<<x_raw<<" y: "<<y_raw<<std::endl;

            iv::lidar::lidarobject * pnew =  xobjarray.add_obj();
            pnew->CopyFrom(*pobj);

        }
    }

    return nAdd;
}