change detection_ndt_matching. add global relocation code. near complete.

src/detection/detection_ndt_matching/globalrelocation.cpp

@@ -1,11 +1,421 @@
 #include "globalrelocation.h"
 
+#include <pcl/filters/voxel_grid.h>
+#include <tf/tf.h>
+#include <pcl/io/pcd_io.h>
+
 GlobalRelocation::GlobalRelocation()
 {
 
 }
 
-void GlobalRelocation::threadReloc(int nThread,cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> * pndt)
+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;
+
+
+    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))
+                {
+                    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);
+                    std::thread * pthread = new std::thread(&GlobalRelocation::threadReloc,this,i,&mndt[i],raw_scan,xReloc);
+                    mthreadcomplete[i] = false;
+                    j++;
+                    if(j == ntracesize)bHaveTask = false;
+                }
+            }
+        }
+        else
+        {
+            for(i=0;i<mnThreadNum;i++)
+            {
+                if((mthreadcomplete[i] == true))
+                {
+
+                }
+                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));
+    }
+
+}
+
+
+void GlobalRelocation::pointreloc(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
+{
+    if(mvectorndtmap.size() > 1)
+    {
+        std::cout<<" only one map, can use relocation."<<std::endl;
+        return;
+    }
+
+    if(mvectorndtmap.size() == 0)
+    {
+        std::cout<<" no map."<<std::endl;
+        return;
+    }
+
+    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;
+    }
+
+    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;
+    }
+
+    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;
+    }
+
+}
+
+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 i;
+        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);
+                }
+            }
+        }
+
+//        for(i=0;i<mvectorseedpoint.size();i++)
+//        {
+//            mvectorseedpoint[i].yaw = mvectorseedpoint[i].yaw + M_PI;
+//            mvectorseedpoint[i].y = mvectorseedpoint[i].y + 10.0;
+//        }
 
+    }
 }

src/detection/detection_ndt_matching/globalrelocation.h

@@ -4,10 +4,31 @@
 #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:
@@ -15,15 +36,39 @@ public:
 
     void 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;
 
-    void threadReloc(int nThread,cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> * pndt);
+    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;
 
+    const double mfSeedDisThresh = 3.0;
+    const int mnCirclDivide  = 20;
+
 };
 
 #endif // GLOBALRELOCATION_H

src/detection/detection_ndt_matching/main.cpp

@@ -21,6 +21,8 @@
 #include "xmlparam.h"
 #include "ivversion.h"
 
+#include "globalrelocation.h"
+
 extern bool gbuseanh;
 
 
@@ -64,6 +66,8 @@ double garm_y = 0.0;
 
 QFile * gpFileLastPos = 0;//Save Last Positin
 
+GlobalRelocation gGlobalRelocation;
+
 /**
  * @brief readtrace read trace
  * @param pFile
@@ -296,6 +300,7 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
                     point_cloud->size());
 
 
+ //   gGlobalRelocation.pointreloc(point_cloud);
 
     point_ndt(point_cloud);
     gndttime = xTime.elapsed();
@@ -575,6 +580,8 @@ int main(int argc, char *argv[])
     LoadTrace();
     std::cout<<"trace size  is "<<gvector_trace.size()<<std::endl;
 
+    gGlobalRelocation.setndtmap(gvector_trace);
+
     gpa = iv::modulecomm::RegisterRecv(gstr_lidarmsgname.data(),ListenPointCloud);
     gpb = iv::modulecomm::RegisterRecv(gstr_gpsmsgname.data(),ListenRaw);
     gpc = iv::modulecomm::RegisterSend(gstr_ndtposmsgname.data(),10000,1);

src/tool/map_collectfromveh/commif.cpp

@@ -3,15 +3,33 @@
 #include "commif.h"
 
 #include <memory>
+#include <thread>
 
 
 static mcommCall  gpgpscall;
+static int64_t gnLastFusionGPSUpdate = 0;
+
 void UpdateGPS(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
 {
     (void)index;
     (void)dt;
     (void)strmemname;
 
+    int64_t nNow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+    if(strncmp(strdata,"fusion_gpsimu",256)!= 0)
+    {
+        if(abs(nNow - gnLastFusionGPSUpdate)<1000)
+        {
+            return;
+        }
+    }
+    else
+    {
+        gnLastFusionGPSUpdate = nNow;
+    }
+
+
     (*gpgpscall)(strdata,nSize);
 
 //    std::shared_ptr<iv::gps::gpsimu> xgpsimu_ptr = std::shared_ptr<iv::gps::gpsimu>(new iv::gps::gpsimu);

src/tool/map_collectfromveh/mainwindow.cpp

@@ -94,6 +94,8 @@ MainWindow::MainWindow(QWidget *parent)
     ptext->setPos(1000,1000);
 
     RegisterRecvGPS((char *)gstr_memname,UpdateGPS);
+    RegisterRecvGPS("fusion_gpsimu",UpdateGPS);
+
 
     mpNowPosItem = new QGraphicsEllipseItem(0-mfNowSize/2.0,0-mfNowSize/2.0,mfNowSize,mfNowSize);
     mpNowPosItem->setBrush(Qt::green);