modularization/src/detection/detection_ndt_matching/globalrelocation.h

#ifndef GLOBALRELOCATION_H
#define GLOBALRELOCATION_H

#include <ndt_cpu/NormalDistributionsTransform.h>

#include <thread>
#include <mutex>
#include "ndt_matching.h"

#define MAX_NDT 100


namespace  iv {
struct Reloc
{
    double x_guess;
    double y_guess;
    double z_guess;
    double yaw_guess;
    double pitch_guess;
    double roll_guess;
    double x_calc;
    double y_calc;
    double z_calc;
    double yaw_calc;
    double pitch_calc;
    double roll_calc;
    double trans_prob;
};
}

class GlobalRelocation
{
public:
    GlobalRelocation();

    iv::Reloc pointreloc(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan);

    void setndtmap(std::vector<iv::ndtmaptrace> & xvectorndtmap);

private:
    cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> mndt[MAX_NDT];

    std::vector<iv::ndtmaptrace> mvectorndtmap;

    std::vector<iv::Reloc> mvectorReloc;
    std::mutex mmutexReloc;

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

    bool mbRelocAvail = false;



    bool mthreadcomplete[MAX_NDT];



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

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

    void RunReloc(std::vector<iv::ndttracepoint> & xtrace,pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan,bool bCheckComplete);

    int mnThreadNum = 8;

    std::thread * mpthread[MAX_NDT];

    const double mfSeedDisThresh = 3.0;
    const int mnCirclDivide  = 20;

};

#endif // GLOBALRELOCATION_H