modularization/src/detection/detection_ndt_matching/globalrelocation.cpp

#include "globalrelocation.h"

#include <pcl/filters/voxel_grid.h>
#include <tf/tf.h>
#include <pcl/io/pcd_io.h>

GlobalRelocation::GlobalRelocation()
{
//    std::cout<<" run hear."<<std::endl;
    if(mnThreadNum > MAX_NDT)mnThreadNum = MAX_NDT;
    int i;
    for(i=0;i<MAX_NDT;i++)mpthread[i] = NULL;
}

void GlobalRelocation::threadtest(int n,iv::Reloc & xReloc)
{

}

void GlobalRelocation::threadReloc(int nThread,cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> * pndt,
                                   pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan,
                                   iv::Reloc  xReloc)
{

    xReloc.x_calc = xReloc.x_guess;
    xReloc.y_calc = xReloc.y_guess;
    xReloc.z_calc = xReloc.z_guess;
    xReloc.yaw_calc = xReloc.yaw_guess;
    xReloc.pitch_calc = 0;// xReloc.pitch_guess;
    xReloc.roll_calc = 0;//xReloc.roll_guess;
    xReloc.trans_prob = 0;


    Eigen::Matrix4f tf_btol;
    Eigen::Translation3f tl_btol(0, 0, 0);                 // tl: translation
    Eigen::AngleAxisf rot_x_btol(0, Eigen::Vector3f::UnitX());  // rot: rotation
    Eigen::AngleAxisf rot_y_btol(0, Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf rot_z_btol(0, Eigen::Vector3f::UnitZ());
    tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();

    double fLastProb = 0.0;
    int i;
    for(i=0;i<50;i++)
    {

        double voxel_leaf_size = 2.0;

        pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan(new pcl::PointCloud<pcl::PointXYZ>());

        pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
        voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
        voxel_grid_filter.setInputCloud(raw_scan);
        voxel_grid_filter.filter(*filtered_scan);

        pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(*filtered_scan));

        pndt->setInputSource(filtered_scan_ptr);

        Eigen::Translation3f init_translation(xReloc.x_calc,xReloc.y_calc, xReloc.z_calc);
        Eigen::AngleAxisf init_rotation_x(xReloc.roll_calc, Eigen::Vector3f::UnitX());
        Eigen::AngleAxisf init_rotation_y(xReloc.pitch_calc, Eigen::Vector3f::UnitY());
        Eigen::AngleAxisf init_rotation_z(xReloc.yaw_calc, Eigen::Vector3f::UnitZ());
        Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x) * tf_btol;

        pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
        pndt->align(*aligned,init_guess);

        bool has_converged;
        int iteration = 0;
        double fitness_score = 0.0;
        double trans_probability = 0.0;
        Eigen::Matrix4f t(Eigen::Matrix4f::Identity());
        Eigen::Matrix4f t2(Eigen::Matrix4f::Identity());


        has_converged = pndt->hasConverged();
        t = pndt->getFinalTransformation();
        iteration = pndt->getFinalNumIteration();
        fitness_score = pndt->getFitnessScore();
        trans_probability = pndt->getTransformationProbability();

        t2 = t * tf_btol.inverse();

        double x,y,z,yaw,pitch,roll;

        tf::Matrix3x3 mat_b;  // base_link
        mat_b.setValue(static_cast<double>(t2(0, 0)), static_cast<double>(t2(0, 1)), static_cast<double>(t2(0, 2)),
                       static_cast<double>(t2(1, 0)), static_cast<double>(t2(1, 1)), static_cast<double>(t2(1, 2)),
                       static_cast<double>(t2(2, 0)), static_cast<double>(t2(2, 1)), static_cast<double>(t2(2, 2)));

        // Update ndt_pose
        x = t2(0, 3);
        y = t2(1, 3);
        z = t2(2, 3);

        //      ndt_pose.x = localizer_pose.x;
        //      ndt_pose.y = localizer_pose.y;
        //      ndt_pose.z = localizer_pose.z;
        mat_b.getRPY(roll, pitch, yaw, 1);

        xReloc.x_calc = x;
        xReloc.y_calc = y;
        xReloc.z_calc = z;
        xReloc.roll_calc = roll;
        xReloc.pitch_calc = pitch;
        xReloc.yaw_calc =yaw;
        xReloc.trans_prob = trans_probability;


 //       std::cout<<"guass x:"<<xReloc.x_guess<<" res:"<<" x: "<<xReloc.x_calc<<" y: "<<xReloc.y_calc<<" prob: "<<xReloc.trans_prob<<" step: "<<i<<std::endl;

        if((i>=10)&&(xReloc.trans_prob<1.0))
        {
            break;
        }

        if((xReloc.trans_prob>3.0) && ((xReloc.trans_prob - fLastProb)<0.01) && (i>20))
        {
            break;
        }

        if((xReloc.trans_prob<1.5) && ((xReloc.trans_prob - fLastProb)<0.01) && (i>20))
        {
            break;
        }

        fLastProb = xReloc.trans_prob;

    }

    mmutexReloc.lock();
    mvectorReloc.push_back(xReloc);
    mmutexReloc.unlock();

    std::cout<<"guass x:"<<xReloc.x_guess<<" y:"<<xReloc.y_guess<<" res:"<<" x: "<<xReloc.x_calc<<" y: "<<xReloc.y_calc<<" prob: "<<xReloc.trans_prob<<std::endl;

    mthreadcomplete[nThread] = true;

}

void GlobalRelocation::RunReloc(std::vector<iv::ndttracepoint> & xtrace,pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan,bool bCheckComplete)
{
    int i;
    bool bComplete = false;
    bool bHaveTask = true;
    int ntracesize = static_cast<int>(xtrace.size());
    int j = 0;
    if(j == ntracesize)bHaveTask = false;
    while(bComplete == false)
    {
        if(bHaveTask)
        {
            for(i=0;i<mnThreadNum;i++)
            {
                if((mthreadcomplete[i] == true) &&(bHaveTask))
                {
                    if(mpthread[i] != NULL)
                    {
                        mpthread[i]->join();
                        delete mpthread[i];
                        mpthread[i] = NULL;
                    }
                    iv::Reloc xReloc;
                    xReloc.x_guess = xtrace[j].x;
                    xReloc.y_guess = xtrace[j].y;
                    xReloc.z_guess = xtrace[j].z;
                    xReloc.yaw_guess = xtrace[j].yaw;
                    xReloc.pitch_guess = 0;
                    xReloc.roll_guess = 0;
                    if(j == ntracesize)bHaveTask = false;
    //                std::thread * pthread = new std::thread(&GlobalRelocation::threadtest,this,i,xReloc);
                    mthreadcomplete[i] = false;
                    mpthread[i] = new std::thread(&GlobalRelocation::threadReloc,this,i,&mndt[i],raw_scan,xReloc);

                    j++;
                    if(j == ntracesize)bHaveTask = false;
                }
            }
        }
        else
        {
            for(i=0;i<mnThreadNum;i++)
            {
                if((mthreadcomplete[i] == true))
                {
                    if(mpthread[i] != NULL)
                    {
                        mpthread[i]->join();
                        delete mpthread[i];
                        mpthread[i] = NULL;
                    }
                }
                else
                {
                    break;
                }
            }
            if(i == mnThreadNum)
            {
                bComplete = true;
                break;
            }
        }

        if(bCheckComplete)
        {
            for(i=0;i<mvectorReloc.size();i++)
            {
                if(mvectorReloc[i].trans_prob >= 3.0)
                {
                    bHaveTask = false;
                    std::cout<<" Complete. "<<std::endl;
                }
            }
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }

}


iv::Reloc GlobalRelocation::pointreloc(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
{
    iv::Reloc xreloczero;
    xreloczero.trans_prob = 0.0;

    if(mvectorndtmap.size() > 1)
    {
        std::cout<<" only one map, can use relocation."<<std::endl;

        return xreloczero;
    }

    if(mvectorndtmap.size() == 0)
    {
        std::cout<<" no map."<<std::endl;
        return xreloczero;
    }

    mvectorReloc.clear();

    int i;
    int j;
    for(i=0;i<mnThreadNum;i++)
    {
        mthreadcomplete[i] = true;
    }

    std::vector<iv::ndttracepoint> xtrace = mvectorseedpoint;

    RunReloc(xtrace,raw_scan,false);

    int nrelocsize = mvectorReloc.size();
    int indexmax = 0;
    double fMaxProb = 0.0;
    for(i=0;i<nrelocsize;i++)
    {
        if(mvectorReloc[i].trans_prob>fMaxProb)
        {
            fMaxProb  = mvectorReloc[i].trans_prob;
            indexmax = i;
        }
    }

    if(fMaxProb > 2.0)
    {

        return mvectorReloc[indexmax];
    }

    std::cout<<" use diff yaw"<<std::endl;

    std::vector<iv::ndttracepoint> xtraceraw = mvectorseedpoint;
    xtrace.clear();

    for(i=0;i<xtraceraw.size();i++)
    {
        iv::ndttracepoint ntp = xtraceraw[i];

        for(j=1;j<mnCirclDivide;j++)
        {
            double yawadd = M_PI*2.0/mnCirclDivide;
            yawadd = yawadd * j;
            iv::ndttracepoint ntpyaw = ntp;
            ntpyaw.yaw = ntp.yaw + yawadd;
            if(ntpyaw.yaw >= 2.0*M_PI)ntpyaw.yaw = ntpyaw.yaw - 2.0*M_PI;
            xtrace.push_back(ntpyaw);
        }
    }


    RunReloc(xtrace,raw_scan,false);

    nrelocsize = mvectorReloc.size();
    indexmax = 0;
    fMaxProb = 0.0;
    for(i=0;i<nrelocsize;i++)
    {
        if(mvectorReloc[i].trans_prob>fMaxProb)
        {
            fMaxProb  = mvectorReloc[i].trans_prob;
            indexmax = i;
        }
    }

    if(fMaxProb > 2.0)
    {
        return mvectorReloc[indexmax];
    }

    std::cout<<" use dis yaw"<<std::endl;

    xtraceraw = mvectorseedpoint;
    xtrace.clear();

    int k;
    for(k=1;k<10;k++)
    {
    for(i=0;i<xtraceraw.size();i++)
    {
        iv::ndttracepoint ntpraw = xtraceraw[i];
        iv::ndttracepoint ntp = ntpraw;
        ntp.x = ntpraw.x + k*mfSeedDisThresh * cos(ntpraw.yaw + M_PI/2.0);
        ntp.y = ntpraw.y + k*mfSeedDisThresh * sin(ntpraw.yaw + M_PI/2.0);
        for(j=0;j<mnCirclDivide;j++)
        {
            double yawadd = M_PI*2.0/mnCirclDivide;
            yawadd = yawadd * j;
            iv::ndttracepoint ntpyaw = ntp;
            ntpyaw.yaw = ntp.yaw + yawadd;
            if(ntpyaw.yaw >= 2.0*M_PI)ntpyaw.yaw = ntpyaw.yaw - 2.0*M_PI;
            xtrace.push_back(ntpyaw);
        }
        ntp = ntpraw;
        ntp.x = ntpraw.x + k*mfSeedDisThresh * cos(ntpraw.yaw - M_PI/2.0);
        ntp.y = ntpraw.y + k*mfSeedDisThresh * sin(ntpraw.yaw - M_PI/2.0);
        for(j=0;j<mnCirclDivide;j++)
        {
            double yawadd = M_PI*2.0/mnCirclDivide;
            yawadd = yawadd * j;
            iv::ndttracepoint ntpyaw = ntp;
            ntpyaw.yaw = ntp.yaw + yawadd;
            if(ntpyaw.yaw >= 2.0*M_PI)ntpyaw.yaw = ntpyaw.yaw - 2.0*M_PI;
            xtrace.push_back(ntpyaw);
        }
    }
    }


    RunReloc(xtrace,raw_scan,true);

    nrelocsize = mvectorReloc.size();
    indexmax = 0;
    fMaxProb = 0.0;
    for(i=0;i<nrelocsize;i++)
    {
        if(mvectorReloc[i].trans_prob>fMaxProb)
        {
            fMaxProb  = mvectorReloc[i].trans_prob;
            indexmax = i;
        }
    }

    if(fMaxProb > 2.0)
    {
        std::cout<<" find pos."<<std::endl;
        return mvectorReloc[indexmax];
    }


    if(mvectorReloc.size() > 0)
    {
        return mvectorReloc[indexmax];
    }


    return xreloczero;


}

void GlobalRelocation::setndtmap(std::vector<iv::ndtmaptrace> & xvectorndtmap)
{
    mvectorndtmap = xvectorndtmap;
    if(mvectorndtmap.size() == 0)
    {
        return;
    }
    int i;
    for(i=0;i<mnThreadNum;i++)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr xmap;

        xmap = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);


        QTime xTime;
        xTime.start();
        if(0 == pcl::io::loadPCDFile(mvectorndtmap[0].mstrpcdpath.data(),*xmap))
        {

        }
        else
        {
            std::cout<<"map size is 0"<<std::endl;
            return;
        }
        mbRelocAvail = true;
        mndt[i].setInputTarget(xmap);
        mthreadcomplete[i] = true;
    }


    int npointsize = mvectorndtmap[0].mvector_trace.size();
    if(npointsize == 0)
    {
        return;
    }
    mvectorseedpoint.clear();
    std::cout<<" load trace seed."<<std::endl;
    iv::ndttracepoint ntp = mvectorndtmap[0].mvector_trace[0];
    mvectorseedpoint.push_back(ntp);

    for(i=1;i<npointsize;i++)
    {
        iv::ndttracepoint ntpnow = mvectorndtmap[0].mvector_trace[i];
        iv::ndttracepoint ntplast = mvectorseedpoint[mvectorseedpoint.size() -1];
        double fdis = sqrt(pow(ntpnow.x - ntplast.x,2) + pow(ntpnow.y - ntplast.y,2));
        double fyawdiff = ntpnow.yaw - ntplast.yaw;
        while(fyawdiff<(-M_PI))fyawdiff = fyawdiff + 2.0*M_PI;
        while(fyawdiff >(M_PI))fyawdiff = fyawdiff - 2.0*M_PI;
        if(fdis>=mfSeedDisThresh)
        {
            mvectorseedpoint.push_back(ntpnow);
        }
        else
        {
            if(fabs(fyawdiff)>0.1)
            {
                mvectorseedpoint.push_back(ntpnow);
            }
            else
            {
                if(i== (npointsize -1))
                {
                    mvectorseedpoint.push_back(ntpnow);
                }
            }
        }
    }

    if(mvectorseedpoint.size()>0)
    {
        const int nfrontadd = 3;
        const int nrearadd  = 3;
        iv::ndttracepoint ntpfirst = mvectorseedpoint[0];
        iv::ndttracepoint ntplast = mvectorseedpoint[mvectorseedpoint.size() -1];
        for(i=0;i<nfrontadd;i++)
        {
            iv::ndttracepoint ntpnow = ntpfirst;
            double foff = (i+1)*mfSeedDisThresh;
            ntpnow.x = ntpfirst.x + foff*cos(ntpfirst.yaw + M_PI);
            ntpnow.y = ntpfirst.y + foff*sin(ntpfirst.yaw + M_PI);
            mvectorseedpoint.insert(mvectorseedpoint.begin(),ntpnow);
        }
        for(i=0;i<nrearadd;i++)
        {
            iv::ndttracepoint ntpnow = ntplast;
            double foff = (i+1)*mfSeedDisThresh;
            ntpnow.x = ntplast.x + foff*cos(ntpfirst.yaw);
            ntpnow.y = ntplast.y + foff*sin(ntpfirst.yaw );
            mvectorseedpoint.push_back(ntpnow);
        }
    }

//        for(i=0;i<mvectorseedpoint.size();i++)
//        {
//            mvectorseedpoint[i].yaw = mvectorseedpoint[i].yaw + M_PI;
//            mvectorseedpoint[i].y = mvectorseedpoint[i].y + 10.0;
//        }


}