add detection_ndt_matching use raw pcl ndt. Because in agx orin, or in x86 ubuntu22.04, the raw ndt is faster than pclomp, 2 times as pclomp . In agx xavier, pclomp is faster than raw ndt, 3 times. If in agx, use matching_gpu or use pclomp. in orin, use raw ndt or pclomp.

src/detection/detection_ndt_matching/detection_ndt_mathcing.pro

@@ -0,0 +1,86 @@
+QT -= gui
+QT += network
+CONFIG += console c++17
+CONFIG -= app_bundle
+
+QMAKE_CXXFLAGS += -std=gnu++17
+
+QMAKE_LFLAGS += -no-pie
+
+QMAKE_CXXFLAGS +=  -g
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += main.cpp \
+    ndt_matching.cpp \
+    ../../include/msgtype/ndtpos.pb.cc \
+    gnss_coordinate_convert.cpp \
+    ../../include/msgtype/imu.pb.cc \
+    ../../include/msgtype/gpsimu.pb.cc \
+    ../../include/msgtype/gps.pb.cc \
+    ../../include/msgtype/ndtgpspos.pb.cc
+
+HEADERS += \
+    ndt_matching.h \
+    ../../include/msgtype/ndtpos.pb.h \
+    gnss_coordinate_convert.h \
+    ../../include/msgtype/imu.pb.h \
+    ../../include/msgtype/gpsimu.pb.h \
+    ../../include/msgtype/gps.pb.h \
+    ../../include/msgtype/ndtgpspos.pb.h
+
+INCLUDEPATH += /opt/ros/melodic/include
+INCLUDEPATH += /opt/ros/kinetic/include
+INCLUDEPATH += /usr/include/pcl-1.8
+INCLUDEPATH += /usr/include/pcl-1.7
+INCLUDEPATH += /usr/include/eigen3
+unix:INCLUDEPATH += /usr/include/pcl-1.10
+INCLUDEPATH += /opt/ros/noetic/include
+
+INCLUDEPATH += $$PWD/../../include/msgtype/
+
+
+
+unix:LIBS += -lboost_thread -lboost_system -lprotobuf
+
+unix:LIBS +=  -lpcl_common\
+        -lpcl_features\
+        -lpcl_filters\
+        -lpcl_io\
+        -lpcl_io_ply\
+        -lpcl_kdtree\
+        -lpcl_keypoints\
+        -lpcl_octree\
+        -lpcl_outofcore\
+        -lpcl_people\
+        -lpcl_recognition\
+        -lpcl_registration\
+        -lpcl_sample_consensus\
+        -lpcl_search\
+        -lpcl_segmentation\
+        -lpcl_surface\
+        -lpcl_tracking\
+        -lpcl_visualization
+
+
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+
+#DEFINES+= TEST_CALCTIME
+
+INCLUDEPATH += $$PWD/../../../include/
+LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault -lndt_gpu  -livexit -livbacktrace
+
+

src/detection/detection_ndt_matching/gnss_coordinate_convert.cpp

@@ -0,0 +1,153 @@
+#include <gnss_coordinate_convert.h>
+
+void gps2xy(double J4, double K4, double *x, double *y)
+{
+    int L4 = (int)((K4 - 1.5) / 3) + 1;
+    double M4 = K4 - (L4 * 3);
+    double N4 = sin(J4 * 3.1415926536 / 180);
+    double O4 = cos(J4 * 3.1415926536 / 180);
+    double P4 = tan(J4 * 3.1415926536 / 180);
+    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
+    double R4 = sqrt(0.006738525414683) * O4;
+    double S4 = sqrt(1 + R4 * R4);
+    double T4 = 6399698.901783 / S4;
+    double U4 = (T4 / S4) / S4;
+    double V4 = O4 * M4 * 3.1415926536 / 180;
+    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
+    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
+    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
+    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
+
+    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
+    *x = 500000 + T4 * V4 * (Y4 + Z4);
+}
+
+//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
+void GaussProjCal(double longitude, double latitude, double *X, double *Y)
+{
+    int ProjNo = 0; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double a, f, e2, ee, NN, T, C, A, M, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    ZoneWide = 6; ////6度带宽
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ProjNo = (int)(longitude / ZoneWide);
+    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI;
+    latitude0 = 0;
+    longitude1 = longitude * iPI; //经度转换为弧度
+    latitude1 = latitude * iPI; //纬度转换为弧度
+    e2 = 2 * f - f * f;
+    ee = e2 * (1.0 - e2);
+    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
+    T = tan(latitude1)*tan(latitude1);
+    C = ee * cos(latitude1)*cos(latitude1);
+    A = (longitude1 - longitude0)*cos(latitude1);
+    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
+        *e2 / 1024)*sin(2 * latitude1)
+        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
+    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
+    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
+        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
+    X0 = 1000000L * (ProjNo + 1) + 500000L;
+    Y0 = 0;
+    xval = xval + X0; yval = yval + Y0;
+    *X = xval;
+    *Y = yval;
+}
+
+
+
+//=======================zhaobo0904
+#define PI  3.14159265358979
+void ZBGaussProjCal(double lon, double lat, double *X, double *Y)
+{
+    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
+    double a = 6378140.0;
+    double e2 = 0.006694384999588;
+    double ep2 = e2/(1-e2);
+
+    // 原点所在经度
+    double lon_origin = 6.0*int(lon/6) + 3.0;
+    lon_origin *= PI / 180.0;
+
+    double k0 = 0.9996;
+
+    // 角度转弧度
+    double lat1 = lat * PI / 180.0;
+    double lon1 = lon * PI / 180.0;
+
+
+    // 经线在该点处的曲率半径,
+    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
+
+
+    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
+    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
+    // 首先计算前四项的系数 a1~a4.
+    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
+    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
+    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
+    double a4 = (35*e2*e2*e2)/3072;
+    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
+
+    // 辅助量
+    double T = tan(lat1)*tan(lat1);
+    double C = ep2*cos(lat1)*cos(lat1);
+    double A = (lon1 - lon_origin)*cos(lat1);
+
+    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
+    *Y = M + N * tan(lat1) * ((A*A)/2 +
+                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
+                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
+
+
+    *Y *= k0;
+    return;
+}
+//==========================================================
+
+
+
+
+
+//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
+{
+    int ProjNo; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ZoneWide = 6; ////6度带宽
+    ProjNo = (int)(X / 1000000L); //查找带号
+    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI; //中央经线
+    X0 = ProjNo * 1000000L + 500000L;
+    Y0 = 0;
+    xval = X - X0; yval = Y - Y0; //带内大地坐标
+    e2 = 2 * f - f * f;
+    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
+    ee = e2 / (1 - e2);
+    M = yval;
+    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
+    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
+                4 * u)
+            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
+    C = ee * cos(fai)*cos(fai);
+    T = tan(fai)*tan(fai);
+    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
+    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
+                                                                                       (fai)*sin(fai)));
+    D = xval / NN;
+    //计算经度(Longitude) 纬度(Latitude)
+    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
+                               *D*D*D*D / 120) / cos(fai);
+    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
+                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
+    //转换为度 DD
+    *longitude = longitude1 / iPI;
+    *latitude = latitude1 / iPI;
+}

src/detection/detection_ndt_matching/gnss_coordinate_convert.h

@@ -0,0 +1,27 @@
+#pragma once
+#ifndef _IV_PERCEPTION_GNSS_CONVERT_
+#define _IV_PERCEPTION_GNSS_CONVERT_
+
+#include <math.h>
+//double nmeaConvert2Lat(string lat)
+//{
+//	Console.WriteLine(lat);
+//	double nmea_d = double.Parse(lat.Substring(0, 2));
+//	double nmea_m = double.Parse(lat.Substring(2, 6));
+//	return nmea_d + nmea_m / 60;
+//}
+//
+//double nmeaConvert2Lon(string lon)
+//{
+//	Console.WriteLine(lon);
+//	double nmea_d = double.Parse(lon.Substring(0, 3));
+//	double nmea_m = double.Parse(lon.Substring(3, 7));
+//	return nmea_d + nmea_m / 60;
+//}
+
+void gps2xy(double , double , double *, double *);
+void GaussProjCal(double longitude, double latitude, double *X, double *Y);
+void ZBGaussProjCal(double longitude, double latitude, double *X, double *Y);
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
+
+#endif // !_IV_PERCEPTION_GNSS_CONVERT_

src/detection/detection_ndt_matching/main.cpp

@@ -0,0 +1,590 @@
+#include <QCoreApplication>
+
+#include "modulecomm.h"
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+
+#include <QTime>
+#include <QDir>
+#include <QFile>
+
+#include <thread>
+
+#include "ivfault.h"
+#include "ivlog.h"
+#include "ndt_matching.h"
+#include "gpsimu.pb.h"
+
+#include "ivexit.h"
+#include "ivbacktrace.h"
+#include "xmlparam.h"
+#include "ivversion.h"
+
+
+QTime gTime;    //a Time function. use calculate elapsed.
+
+int gndttime;    //ndt calculate time
+
+void * gpa,* gpb ,* gpc, * gpd;   //Modulecomm pointer
+
+iv::Ivfault *gfault = nullptr;   //fault diag
+iv::Ivlog *givlog = nullptr;   //log
+
+std::vector<iv::ndtmaptrace> gvector_trace;  //trace and map origin. use shift map
+
+iv::gpspos glastndtgpspos;    //last ndt pos(convert to gps)
+iv::gpspos gcurndtgpspos;     //cur ndt pos(conver to gps)
+iv::gpspos gcurgpsgpspos;    //cur gps pos
+bool gbGPSFix = false;   //GPS is Fix. If 300 times report rtk 6.
+int64_t gnLastGPSTime = 0; //ms
+
+std::thread * gpthread;   //State thread pointer.
+
+std::string gstr_lidarmsgname;   //lidar message name
+std::string gstr_gpsmsgname;   // gps message name
+std::string gstr_ndtposmsgname;  //ndtpos message name
+std::string gstr_ndtgpsposmsgname;   //gpspos message name
+std::string gstr_ndtmainsensormsgname;
+std::string gstr_arm_x;
+std::string gstr_arm_y;
+
+std::string gstr_yawcorrect;   //lidar yaw correct.
+std::string gstr_heightcorrect;  //gps height correct.
+double gyawcorrect = 0.0;        //lidar yaw correct.
+double gheightcorrect = 0.0;      //gps height correct.
+
+std::string gstr_savelastpos; //set save last pos. default true
+bool gbSaveLastPos = true;
+
+double garm_x = 0.0;
+double garm_y = 0.0;
+
+QFile * gpFileLastPos = 0;//Save Last Positin
+
+/**
+ * @brief readtrace read trace
+ * @param pFile
+ * @return
+ */
+std::vector<iv::ndttracepoint> readtrace(QFile * pFile)
+{
+    std::vector<iv::ndttracepoint> ntp;
+    QByteArray ba;
+    ba = pFile->readLine();
+
+    do
+    {
+        QString str;
+        str = ba;
+        QStringList strlist;
+        strlist = str.split("\t");
+        if(strlist.size() == 6)
+        {
+            iv::ndttracepoint tp;
+            QString strx;
+            int j;
+            double fv[6];
+            for(j=0;j<6;j++)
+            {
+                strx = strlist.at(j);
+                fv[j] = strx.toDouble();
+            }
+            tp.x = fv[0];
+            tp.y = fv[1];
+            tp.z = fv[2];
+            tp.pitch = fv[3];
+            tp.roll = fv[4];
+            tp.yaw = fv[5];
+            ntp.push_back(tp);
+        }
+        else
+        {
+            qDebug("len is %d, %d",ba.length(),strlist.size());
+        }
+        ba = pFile->readLine();
+    }while(ba.length()>0);
+
+    return ntp;
+}
+
+/**
+ * @brief readndtorigin read .pcd file's origin gps position.
+ * @param pFile
+ * @param pnori
+ * @return
+ */
+int readndtorigin(QFile * pFile,iv::gpspos * pnori)
+{
+    int nrtn = -1;
+    QByteArray ba;
+    ba = pFile->readLine();
+    QString str;
+    str = ba;
+    QStringList strlist;
+    strlist = str.split("\t");
+    if(strlist.size() == 6)
+    {
+        QString xstr[6];
+        int i;
+        for(i=0;i<6;i++)xstr[i] = strlist.at(i);
+        pnori->lon = xstr[0].toDouble();
+        pnori->lat = xstr[1].toDouble();
+        pnori->height = xstr[2].toDouble();
+        pnori->heading = xstr[3].toDouble();
+        pnori->pitch = xstr[4].toDouble();
+        pnori->roll = xstr[5].toDouble();
+        nrtn = 0;
+    }
+    return nrtn;
+
+}
+
+/**
+ * @brief LoadLastPos Load last position
+ * @param strlastposfilepath
+ */
+static void LoadLastPos(const char * strlastposfilepath)
+{
+    QFile * xFile = new QFile();
+    xFile->setFileName(strlastposfilepath);
+    if(xFile->open(QIODevice::ReadWrite))
+    {
+        int nrtn = readndtorigin(xFile,&glastndtgpspos);
+        if(nrtn < 0)
+        {
+            givlog->warn("load last pos fail.");
+        }
+        if(gbSaveLastPos)gpFileLastPos = xFile;
+        else
+        {
+            gpFileLastPos = 0;
+            xFile->close();
+        }
+    }
+}
+
+/**
+ * @brief LoadTrace
+ */
+static void LoadTrace()
+{
+    std::string strpath = getenv("HOME");
+    strpath = strpath + "/map/";
+
+    std::string strlastposfile = strpath;
+    strlastposfile = strlastposfile + "lastpos.txt";
+    LoadLastPos(strlastposfile.data());
+
+    QString strpathdir = strpath.data();
+    QDir dir(strpath.data());
+    QStringList xfilter;
+    xfilter<<"*.pcd";
+    foreach(QString strfilename,dir.entryList(xfilter,QDir::Files))
+    {
+        qDebug(strfilename.toLatin1().data());
+        QString stroripath;
+        QString strtracepath;
+        QString strpcdpath;
+        stroripath = strfilename;
+        stroripath = stroripath.left(stroripath.length() - 4);
+        stroripath = strpathdir +  stroripath + ".ori";
+        strtracepath = strfilename;
+        strtracepath = strtracepath.left(strtracepath.length() - 4);
+        strtracepath = strpathdir +  strtracepath + ".txt";
+        strpcdpath = strfilename;
+        strpcdpath = strpathdir + strpcdpath;
+        QFile xFileori,xFile,xFilepcd;
+
+        xFileori.setFileName(stroripath);
+        xFilepcd.setFileName(strpcdpath);
+        xFile.setFileName(strtracepath);
+        if(xFileori.exists() && xFile.exists())
+        {
+            qDebug("file name ok.");
+            if(xFile.open(QIODevice::ReadOnly) && xFileori.open(QIODevice::ReadOnly))
+            {
+                std::vector<iv::ndttracepoint> xv =  readtrace(&xFile);
+                iv::gpspos xnori;
+                int nrtn = readndtorigin(&xFileori,&xnori);
+                if((xv.size() > 0)&&(nrtn == 0))
+                {
+                    iv::ndtmaptrace nmt;
+                    nmt.mvector_trace = xv;
+                    nmt.mstrpcdpath = strpcdpath.toLatin1().data();
+                    nmt.mndtorigin = xnori;
+                    gvector_trace.push_back(nmt);
+                    qDebug("this is ok.");
+                }
+
+            }
+            xFile.close();
+            xFileori.close();
+
+        }
+        else
+        {
+            qDebug(" file not ok.");
+        }
+    }
+    return;
+}
+
+int gnothavedatatime = 0;
+/**
+ * @brief ListenPointCloud
+ * @param strdata
+ * @param nSize
+ * @param index
+ * @param dt
+ * @param strmemname
+ */
+void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    QTime xTime;
+    xTime.start();
+
+    gnothavedatatime = 0;
+    if(nSize <=16)return;
+    unsigned int * pHeadSize = (unsigned int *)strdata;
+    if(*pHeadSize > nSize)
+    {
+        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
+    }
+
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZ>());
+    int nNameSize;
+    nNameSize = *pHeadSize - 4-4-8;
+    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
+    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
+    point_cloud->header.frame_id = strName;
+    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
+    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
+    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
+    int i;
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
+    for(i=0;i<nPCount;i++)
+    {
+        pcl::PointXYZ xp;
+        xp.x = p->y;
+        xp.y = p->x * (-1.0);
+        xp.z = p->z;
+
+        if(gyawcorrect != 0.0)
+        {
+            double yaw =  gyawcorrect;
+            double x1,y1;
+            x1 = xp.x;
+            y1 = xp.y;
+            xp.x = x1*cos(yaw) -y1*sin(yaw);
+            xp.y = x1*sin(yaw) + y1*cos(yaw);
+
+        }
+
+ //       xp.intensity = p->intensity;
+        point_cloud->push_back(xp);
+        p++;
+    }
+
+    givlog->verbose("point cloud size is %d",
+                    point_cloud->size());
+
+
+
+    point_ndt(point_cloud);
+    gndttime = xTime.elapsed();
+    givlog->verbose("ndt time is %d, gtime is %d", xTime.elapsed(), gTime.elapsed());
+ //   return;
+
+
+ //   gw->UpdatePointCloud(point_cloud);
+ //   DBSCAN_PC(point_cloud);
+
+    gfault->SetFaultState(0, 0, "ok");
+}
+
+/**
+ * @brief ListenMapUpdate
+ * @param strdata
+ * @param nSize
+ * @param index
+ * @param dt
+ * @param strmemname
+ */
+void ListenMapUpdate(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    char * strpath = new char[nSize+1];
+    memcpy(strpath,strdata,nSize);
+    strpath[nSize] = 0;
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr mapx = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);;
+    if(0 == pcl::io::loadPCDFile(strpath,*mapx))
+    {
+        int size = mapx->size();
+        givlog->verbose("mapx size = %d", size);
+        ndt_match_SetMap(mapx);
+
+    }
+
+    gfault->SetFaultState(0, 0, "ok");
+    delete strpath;
+}
+
+/**
+ * @brief ListenNDTRunstate
+ * @param strdata
+ * @param nSize
+ * @param index
+ * @param dt
+ * @param strmemname
+ */
+void ListenNDTRunstate(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    bool bState;
+    if(nSize < sizeof(bool))
+    {
+        gfault->SetFaultState(1, 0, "ListenNDTRunstate data size is small");
+        givlog->error("ListenNDTRunstate data size is small");
+        return;
+    }
+    memcpy(&bState,strdata,sizeof(bool));
+
+    SetRunState(bState);
+    gfault->SetFaultState(0, 0, "ok");
+}
+
+/**
+ * @brief pausecomm
+ */
+void pausecomm()
+{
+    iv::modulecomm::PauseComm(gpa);
+    iv::modulecomm::PauseComm(gpb);
+    iv::modulecomm::PauseComm(gpc);
+    iv::modulecomm::PauseComm(gpd);
+}
+
+/**
+ * @brief continuecomm
+ */
+void continuecomm()
+{
+    iv::modulecomm::ContintuComm(gpa);
+    iv::modulecomm::ContintuComm(gpb);
+    iv::modulecomm::ContintuComm(gpc);
+    iv::modulecomm::ContintuComm(gpd);
+}
+
+/**
+ * @brief ListenRaw
+ * @param strdata
+ * @param nSize
+ * @param index
+ * @param dt
+ * @param strmemname
+ */
+void ListenRaw(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    static unsigned int nFixCount = 0;
+    iv::gps::gpsimu xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ListenRaw Parse error."<<std::endl;
+    }
+
+    gcurgpsgpspos.lat = xgpsimu.lat();
+    gcurgpsgpspos.lon = xgpsimu.lon();
+    gcurgpsgpspos.height = xgpsimu.height() + gheightcorrect;
+    gcurgpsgpspos.heading = xgpsimu.heading();
+    gcurgpsgpspos.pitch = xgpsimu.pitch();
+    gcurgpsgpspos.roll = xgpsimu.roll();
+    gcurgpsgpspos.ve = xgpsimu.ve();
+    gcurgpsgpspos.vn = xgpsimu.vn();
+
+    givlog->verbose("gps lat:%11.7f lon:%11.7f heading:%11.7f height:%6.3f rtk:%d",
+                    xgpsimu.lat(),xgpsimu.lon(),xgpsimu.heading(),
+                    xgpsimu.height(),xgpsimu.rtk_state());
+
+    if(xgpsimu.has_acce_x())
+    {
+        setuseimu(true);
+        imu_update(xgpsimu.time(),
+                   xgpsimu.roll(),xgpsimu.pitch(),
+                   xgpsimu.heading(),xgpsimu.acce_x(),
+                   xgpsimu.acce_y(),xgpsimu.acce_z());
+    }
+
+
+    if(xgpsimu.rtk_state() == 6)
+    {
+        nFixCount++;
+    }
+    else
+    {
+        nFixCount = 0;
+    }
+
+    if(nFixCount < 300)gbGPSFix = false;
+    else gbGPSFix = true;
+
+    gnLastGPSTime = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+}
+
+
+bool gbstate = true;
+
+/**
+ * @brief statethread Monitor ndt state thread.
+ */
+void statethread()
+{
+    int nstate = 0;
+    int nlaststate = 0;
+    while(gbstate)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        if(gnothavedatatime < 100000) gnothavedatatime++;
+
+        if(gnothavedatatime < 100)
+        {
+            nstate = 0;
+        }
+        if(gnothavedatatime > 1000)
+        {
+            nstate = 1;
+        }
+        if(gnothavedatatime > 6000)
+        {
+            nstate = 2;
+        }
+        if(nstate != nlaststate)
+        {
+            nlaststate = nstate;
+            switch (nstate) {
+            case 0:
+                givlog->info("ndt matching  is ok");
+                gfault->SetFaultState(0,0,"data is ok.");
+                break;
+            case 1:
+                givlog->warn("more than 10 seconds not have lidar pointcloud.");
+                gfault->SetFaultState(1,1,"more than 10 seconds not have lidar pointcloud.");
+                break;
+            case 2:
+                givlog->error("more than 60 seconds not have lidar pointcloud.");
+                gfault->SetFaultState(2,2,"more than 60 seconds not have lidar pointcloud.");
+                break;
+            default:
+                break;
+            }
+        }
+    }
+}
+
+/**
+ * @brief exitfunc when receive exit command.
+ */
+void exitfunc()
+{
+    std::cout<<"enter exitfunc."<<std::endl;
+    gbstate = false;
+    gpthread->join();
+    iv::modulecomm::Unregister(gpa);
+    iv::modulecomm::Unregister(gpb);
+    iv::modulecomm::Unregister(gpc);
+    iv::modulecomm::Unregister(gpd);
+    if(gpFileLastPos != 0)gpFileLastPos->close();
+    std::cout<<"Complete exitfunc."<<std::endl;
+}
+
+int main(int argc, char *argv[])
+{
+    showversion("detection_ndt_matching_gpu_multi");
+    QCoreApplication a(argc, argv);
+
+    QString strpath = QCoreApplication::applicationDirPath();
+
+   if(argc < 2)
+       strpath = strpath + "/detection_ndt_matching_gpu_multi.xml";
+   else
+       strpath = argv[1];
+   std::cout<<strpath.toStdString()<<std::endl;
+
+   iv::xmlparam::Xmlparam xparam(strpath.toStdString());
+
+   gstr_lidarmsgname = xparam.GetParam("lidarmsg","lidar_pc");
+   gstr_gpsmsgname = xparam.GetParam("gpsmsg","hcp2_gpsimu");
+   gstr_ndtposmsgname = xparam.GetParam("ndtposmsg","ndtpos");
+   gstr_ndtgpsposmsgname = xparam.GetParam("ndtgpsposmsg","ndtgpspos");
+   gstr_ndtmainsensormsgname = xparam.GetParam("mainsensor","gps");
+
+   gstr_yawcorrect = xparam.GetParam("AngleCorrect","0.0");
+   gstr_heightcorrect = xparam.GetParam("HeightCorrect","0.0");
+   gstr_savelastpos = xparam.GetParam("SaveLastPos","true");
+
+   gstr_arm_x = xparam.GetParam("Arm_LidarGPSBase_x","0.0");
+   gstr_arm_y = xparam.GetParam("Arm_LidarGPSBase_y","0.0");
+
+   gyawcorrect = atof(gstr_yawcorrect.data()) * M_PI/180.0;
+   gheightcorrect = atof(gstr_heightcorrect.data());
+
+   if(gstr_savelastpos == "true")gbSaveLastPos = true;
+   else gbSaveLastPos = false;
+
+   garm_x = atof(gstr_arm_x.data());
+   garm_y = atof(gstr_arm_y.data());
+
+
+   std::cout<<"lidar message is "<<gstr_lidarmsgname<<std::endl;
+   std::cout<<"gps message is "<<gstr_gpsmsgname<<std::endl;
+   std::cout<<"ndtpos message is "<<gstr_ndtposmsgname<<std::endl;
+   std::cout<<"ndtgpspos message is "<<gstr_ndtgpsposmsgname<<std::endl;
+   std::cout<<"AngleCorrect  is "<<gstr_yawcorrect<<std::endl;
+   std::cout<<"HeightCorrect is "<<gstr_heightcorrect<<std::endl;
+   std::cout<<"SaveLastPos is "<<gstr_savelastpos<<std::endl;
+   std::cout<<"Arm_LidarGPSBase_x is "<<gstr_arm_x<<std::endl;
+   std::cout<<"Arm_LidarGPSBase_x is "<<gstr_arm_y<<std::endl;
+
+
+    gfault = new iv::Ivfault("detection_ndt_matching_gpu");
+    givlog = new iv::Ivlog("detection_ndt_matching_gpu");
+
+    gfault->SetFaultState(0,0,"Initialize.");
+
+    glastndtgpspos.lat = 39;
+    glastndtgpspos.lon = 119;
+    glastndtgpspos.heading = 0;
+    glastndtgpspos.height = 0;
+    glastndtgpspos.pitch = 0;
+    glastndtgpspos.roll = 0;
+    gcurndtgpspos = glastndtgpspos;
+
+    iv::gpspos xpos = glastndtgpspos;
+    xpos.lat = 39.1;
+    xpos.heading = 90;
+    pose xp = CalcPose(glastndtgpspos,xpos);
+    iv::gpspos xx = PoseToGPS(glastndtgpspos,xp);
+    LoadTrace();
+    std::cout<<"trace size  is "<<gvector_trace.size()<<std::endl;
+
+    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);
+    gpd = iv::modulecomm::RegisterSend(gstr_ndtgpsposmsgname.data(),10000,1);
+    ndt_match_Init_nomap();
+
+    iv::ivexit::RegIVExitCall(exitfunc);
+    RegisterIVBackTrace();
+
+    gpthread = new std::thread(statethread);
+//    gpb = iv::modulecomm::RegisterRecv("ndt_mappath",ListenMapUpdate);
+ //   gpc = iv::modulecomm::RegisterRecv("ndt_runstate",ListenNDTRunstate);
+
+
+
+    return a.exec();
+}

src/detection/detection_ndt_matching/ndt_matching.cpp

@@ -0,0 +1,1818 @@
+/*
+ * Copyright 2015-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/*
+ Localization program using Normal Distributions Transform
+
+ Yuki KITSUKAWA
+ */
+
+#include <pthread.h>
+#include <chrono>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <sstream>
+#include <string>
+
+#include <thread>
+
+#include <boost/filesystem.hpp>
+
+
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+//#include <ndt_cpu/NormalDistributionsTransform.h>
+#include <pcl/registration/ndt.h>
+
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/registration/ndt.h>
+#include <pcl/registration/gicp.h>
+#include <pcl/filters/voxel_grid.h>
+//#include <pcl/visualization/pcl_visualizer.h>
+
+#include <QFile>
+
+//#include <ndt_gpu/NormalDistributionsTransform.h>
+#include "ndtpos.pb.h"
+#include "ndtgpspos.pb.h"
+
+#include <pcl_ros/point_cloud.h>
+#include <pcl_ros/transforms.h>
+
+
+
+#include "ndt_matching.h"
+
+#include "modulecomm.h"
+
+
+
+#include "gnss_coordinate_convert.h"
+
+//the following are UBUNTU/LINUX ONLY terminal color codes.
+#define RESET   "\033[0m"
+#define BLACK   "\033[30m"      /* Black */
+#define RED     "\033[31m"      /* Red */
+#define GREEN   "\033[32m"      /* Green */
+#define YELLOW  "\033[33m"      /* Yellow */
+#define BLUE    "\033[34m"      /* Blue */
+#define MAGENTA "\033[35m"      /* Magenta */
+#define CYAN    "\033[36m"      /* Cyan */
+#define WHITE   "\033[37m"      /* White */
+#define BOLDBLACK   "\033[1m\033[30m"      /* Bold Black */
+#define BOLDRED     "\033[1m\033[31m"      /* Bold Red */
+#define BOLDGREEN   "\033[1m\033[32m"      /* Bold Green */
+#define BOLDYELLOW  "\033[1m\033[33m"      /* Bold Yellow */
+#define BOLDBLUE    "\033[1m\033[34m"      /* Bold Blue */
+#define BOLDMAGENTA "\033[1m\033[35m"      /* Bold Magenta */
+#define BOLDCYAN    "\033[1m\033[36m"      /* Bold Cyan */
+#define BOLDWHITE   "\033[1m\033[37m"      /* Bold White */
+
+
+#define PREDICT_POSE_THRESHOLD 0.5
+
+#define Wa 0.4
+#define Wb 0.3
+#define Wc 0.3
+
+#include "ivfault.h"
+#include "ivlog.h"
+
+extern iv::Ivfault *gfault ;
+extern iv::Ivlog *givlog;
+
+static int gindex = 0;
+iv::lidar_pose glidar_pose;
+
+void * gpset;
+void * gppose;
+
+static bool g_hasmap = false;
+
+enum class MethodType
+{
+  PCL_GENERIC = 0,
+  PCL_ANH = 1,
+  PCL_ANH_GPU = 2,
+  PCL_OPENMP = 3,
+};
+static MethodType _method_type = MethodType::PCL_GENERIC;
+
+static pose initial_pose, predict_pose, predict_pose_imu, predict_pose_odom, predict_pose_imu_odom, previous_pose,
+    ndt_pose, current_pose, current_pose_imu, current_pose_odom, current_pose_imu_odom, localizer_pose;
+
+static double offset_x, offset_y, offset_z, offset_yaw;  // current_pos - previous_pose
+static double offset_imu_x, offset_imu_y, offset_imu_z, offset_imu_roll, offset_imu_pitch, offset_imu_yaw;
+static double offset_odom_x, offset_odom_y, offset_odom_z, offset_odom_roll, offset_odom_pitch, offset_odom_yaw;
+static double offset_imu_odom_x, offset_imu_odom_y, offset_imu_odom_z, offset_imu_odom_roll, offset_imu_odom_pitch,
+    offset_imu_odom_yaw;
+
+// Can't load if typed "pcl::PointCloud<pcl::PointXYZRGB> map, add;"
+static pcl::PointCloud<pcl::PointXYZ> map, add;
+
+// If the map is loaded, map_loaded will be 1.
+static int map_loaded = 0;
+static int _use_gnss = 1;
+static int init_pos_set = 0;
+
+static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
+
+
+
+static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr glocalndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+
+// Default values
+static int max_iter = 30;        // Maximum iterations
+static float ndt_res = 1.0;      // Resolution
+static double step_size = 0.1;   // Step size
+static double trans_eps = 0.01;  // Transformation epsilon
+
+
+static double exe_time = 0.0;
+static bool has_converged;
+static int iteration = 0;
+static double fitness_score = 0.0;
+static double trans_probability = 0.0;
+
+static double diff = 0.0;
+static double diff_x = 0.0, diff_y = 0.0, diff_z = 0.0, diff_yaw;
+
+static double current_velocity = 0.0, previous_velocity = 0.0, previous_previous_velocity = 0.0;  // [m/s]
+static double current_velocity_x = 0.0, previous_velocity_x = 0.0;
+static double current_velocity_y = 0.0, previous_velocity_y = 0.0;
+static double current_velocity_z = 0.0, previous_velocity_z = 0.0;
+// static double current_velocity_yaw = 0.0, previous_velocity_yaw = 0.0;
+static double current_velocity_smooth = 0.0;
+
+static double current_velocity_imu_x = 0.0;
+static double current_velocity_imu_y = 0.0;
+static double current_velocity_imu_z = 0.0;
+
+static double current_accel = 0.0, previous_accel = 0.0;  // [m/s^2]
+static double current_accel_x = 0.0;
+static double current_accel_y = 0.0;
+static double current_accel_z = 0.0;
+// static double current_accel_yaw = 0.0;
+
+static double angular_velocity = 0.0;
+
+static int use_predict_pose = 0;
+
+
+static std::chrono::time_point<std::chrono::system_clock> matching_start, matching_end;
+
+
+static int _queue_size = 1000;
+
+
+static double predict_pose_error = 0.0;
+
+static double _tf_x, _tf_y, _tf_z, _tf_roll, _tf_pitch, _tf_yaw;
+static Eigen::Matrix4f tf_btol;
+
+static std::string _localizer = "velodyne";
+static std::string _offset = "linear";  // linear, zero, quadratic
+
+
+
+static bool _get_height = false;
+static bool _use_local_transform = false;
+static bool _use_imu = false;
+static bool _use_odom = false;
+static bool _imu_upside_down = false;
+static bool _output_log_data = false;
+
+static std::string _imu_topic = "/imu_raw";
+
+static std::ofstream ofs;
+static std::string filename;
+
+//static sensor_msgs::Imu imu;
+//static nav_msgs::Odometry odom;
+
+// static tf::TransformListener local_transform_listener;
+static tf::StampedTransform local_transform;
+
+static unsigned int points_map_num = 0;
+
+pthread_mutex_t mutex;
+
+pthread_mutex_t mutex_read;
+
+pthread_mutex_t mutex_pose;
+
+bool gbNeedGPSUpdateNDT = false;
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr local_map_ptr;
+pcl::PointCloud<pcl::PointXYZ>::Ptr global_map_ptr;
+
+pose glastmappose;
+
+static double imu_angular_velocity_x=0;
+static double imu_angular_velocity_y=0;
+static double imu_angular_velocity_z=0;
+static double imu_linear_acceleration_x=0;
+static double imu_linear_acceleration_y=0;
+static double imu_linear_acceleration_z =0;
+
+extern QFile * gpFileLastPos;//Save Last Positin
+static bool gbFileNDTUpdate;
+
+extern double garm_x ;
+extern double garm_y ;
+
+#include <QDateTime>
+
+//cv::Mat gmatimage;
+void ListenPoseSet(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::lidar::ndtpos pos;
+
+    if(false == pos.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<" ndtpos parse error."<<std::endl;
+        return;
+    }
+
+    SetCurPose(pos.pose_x(),pos.pose_y(),pos.pose_yaw(),pos.pose_z(),pos.pose_pitch(),pos.pose_roll());
+
+
+}
+
+static void SetLocalMap()
+{
+    pcl::PointCloud<pcl::PointXYZ>::Ptr local_ptr(new pcl::PointCloud<pcl::PointXYZ>());
+
+    int nSize = global_map_ptr->size();
+
+    int i;
+    for(i=0;i<nSize;i++)
+    {
+        pcl::PointXYZ xp = global_map_ptr->at(i);
+        if(sqrt(pow(xp.x - current_pose.x,2)+pow(xp.y-current_pose.y,2)+pow(xp.z-current_pose.z,2))<30)
+        {
+            local_ptr->push_back(xp);
+        }
+    }
+
+    glastmappose = current_pose;
+
+    local_map_ptr = local_ptr;
+    std::cout<<"current map size is "<<local_map_ptr->size()<<std::endl;
+}
+
+static bool gbNDTRun = true;
+
+static bool gbGFRun = true;
+static bool gbLMRun= true;
+static std::thread * gpmapthread, * gpgpsfixthread,*gpsavelastthread;
+static bool gbNeedUpdate = false;
+
+extern iv::gpspos glastndtgpspos;
+extern iv::gpspos gcurndtgpspos;
+extern iv::gpspos gcurgpsgpspos;
+extern bool gbGPSFix;
+extern int64_t gnLastGPSTime;
+
+static bool gbgpsupdatendt = false;
+
+static int gusemapindex = -1;
+static int gcurmapindex = -1;
+
+extern std::vector<iv::ndtmaptrace> gvector_trace;
+
+extern void * gpa,* gpb ,* gpc, * gpd;
+
+iv::gpspos PoseToGPS(iv::gpspos xori,pose xpose)
+{
+    double x_o,y_o;
+    GaussProjCal(xori.lon,xori.lat,&x_o,&y_o);
+    double lon,lat;
+    double hdg_o = (90 - xori.heading)*M_PI/180.0;
+    double rel_x = xpose.x * cos(hdg_o) - xpose.y * sin(hdg_o);
+    double rel_y = xpose.x * sin(hdg_o) + xpose.y * cos(hdg_o);
+    GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
+    double hdg_c = hdg_o + xpose.yaw;
+
+    hdg_c = M_PI/2.0 - hdg_c;
+    double heading = hdg_c * 180.0/M_PI;
+    while(heading < 0)heading = heading + 360;
+    while(heading >360)heading = heading - 360;
+    iv::gpspos xgpspos;
+    xgpspos.lon = lon;
+    xgpspos.lat = lat;
+    xgpspos.height = xpose.z + xori.height;
+    xgpspos.heading = heading;
+    xgpspos.pitch = xpose.pitch + xori.pitch;
+    xgpspos.roll  = xpose.roll + xori.roll;
+    xgpspos.ve = xpose.vx * cos(hdg_o) - xpose.vy * sin(hdg_o);
+    xgpspos.vn = xpose.vx * sin(hdg_o) + xpose.vy * cos(hdg_o);
+
+    if((garm_x != 0)||(garm_y != 0)) //arm use to Convert pos to GPS Posision
+    {
+        GaussProjCal(xgpspos.lon,xgpspos.lat,&x_o,&y_o);
+        hdg_o = (90 - xgpspos.heading)*M_PI/180.0;
+        rel_x = garm_x *(-1) * cos(hdg_o) - garm_y *(-1) * sin(hdg_o);
+        rel_y = garm_x *(-1) * sin(hdg_o) + garm_y *(-1) * cos(hdg_o);
+        GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
+        xgpspos.lon = lon;
+        xgpspos.lat = lat;
+    }
+    return xgpspos;
+
+}
+
+pose CalcPose(iv::gpspos xori, iv::gpspos xcur)
+{
+    double lon,lat;
+    if((garm_x != 0)||(garm_y != 0)) //arm use to Convert pos to GPS Posision
+    {
+        double x_o,y_o;
+        GaussProjCal(xcur.lon,xcur.lat,&x_o,&y_o);
+        double hdg_o = (90 - xcur.heading)*M_PI/180.0;
+        double rel_x = garm_x *(1) * cos(hdg_o) - garm_y *(1) * sin(hdg_o);
+        double rel_y = garm_x *(1) * sin(hdg_o) + garm_y *(1) * cos(hdg_o);
+        GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
+        xcur.lon = lon;
+        xcur.lat = lat;
+    }
+    double x_o,y_o,hdg_o;
+    double x_c,y_c,hdg_c;
+    GaussProjCal(xori.lon,xori.lat,&x_o,&y_o);
+    GaussProjCal(xcur.lon,xcur.lat,&x_c,&y_c);
+    double dis = sqrt(pow(x_c-x_o,2)+ pow(y_c-y_o,2));
+    double rel_x0,rel_y0;
+    rel_x0 = x_c -x_o;
+    rel_y0 = y_c -y_o;
+    double rel_x,rel_y;
+
+    hdg_o = (90 - xori.heading)*M_PI/180.0;
+    hdg_c = (90 - xcur.heading)*M_PI/180.0;
+    rel_x = rel_x0 * cos(-hdg_o) - rel_y0 * sin(-hdg_o);
+    rel_y = rel_x0 * sin(-hdg_o) + rel_y0 * cos(-hdg_o);
+
+
+    double rel_hdg;
+    rel_hdg = hdg_c - hdg_o;
+    pose posex;
+    posex.x = rel_x;
+    posex.y = rel_y;
+    posex.z = xcur.height - xori.height;
+    posex.pitch = xcur.pitch - xori.pitch;
+    posex.roll = xcur.roll - xori.roll;
+    posex.yaw = rel_hdg;
+
+    posex.vx = xcur.ve * cos(-hdg_o) - xcur.vn * sin(-hdg_o);
+    posex.vy = xcur.ve * sin(-hdg_o) + xcur.vn * cos(-hdg_o);
+
+    return posex;
+
+}
+
+static double gettracedis(int index,pose posex)
+{
+    double fdismin = 1000000.;
+    double zdiff = 0;
+    int neari;
+    if(index < 0)return 1000000.0;
+    if(index>= gvector_trace.size())
+    {
+        return 1000000.0;
+    }
+    int i;
+    std::vector<iv::ndttracepoint> vt = gvector_trace.at(index).mvector_trace;
+    int nsize = vt.size();
+//    std::cout<<"size is "<<nsize<<std::endl;
+    for(i=0;i<nsize;i++)
+    {
+        double fdis = sqrt(pow(posex.x - vt.at(i).x,2) + pow(posex.y - vt.at(i).y,2));
+//        std::cout<<"fdis is "<<fdis<<std::endl;
+        if((fdis < fdismin) &&(fabs(posex.z - vt.at(i).z)<5))
+        {
+            fdismin = fdis;
+            neari = i;
+            zdiff = fabs(posex.z - vt.at(i).z);
+        }
+    }
+
+    return fdismin;
+}
+
+static void getmindistrace(int & index, double & ftracedis)
+{
+    double fdismin = 1000000.0;
+    int xindexmin = -1;
+    int i;
+    int nsize = gvector_trace.size();
+
+
+    for(i=0;i<nsize;i++)
+    {
+        pose posex = CalcPose(gvector_trace[i].mndtorigin,gcurndtgpspos);
+        double fdis = gettracedis(i,posex);
+        if(fdis<fdismin)
+        {
+            fdismin = fdis;
+            xindexmin = i;
+        }
+    }
+
+    if(xindexmin != -1)
+    {
+        ftracedis = fdismin;
+        index = xindexmin;
+    }
+    else
+    {
+        index = -1;
+        ftracedis = 100000.0;
+    }
+}
+
+#include <QTime>
+
+extern void pausecomm();
+extern void continuecomm();
+static void UseMap(int index)
+{
+    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(gvector_trace.at(index).mstrpcdpath.data(),*xmap))
+    {
+
+    }
+    else
+    {
+        std::cout<<"map size is 0"<<std::endl;
+        return;
+    }
+
+    qDebug("load map time is %d",xTime.elapsed());
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*xmap));
+//    gvectoranh.push_back(new_anh_gpu_ndt_ptr);
+
+    qDebug("ndt load finish time is %d",xTime.elapsed());
+
+    gcurmapindex = index;
+
+
+
+    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr localndtraw(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+
+
+    localndtraw->setResolution(ndt_res);
+    localndtraw->setInputTarget(map_ptr);
+    localndtraw->setMaximumIterations(max_iter);
+    localndtraw->setStepSize(step_size);
+    localndtraw->setTransformationEpsilon(trans_eps);
+
+    pthread_mutex_lock(&mutex);
+//        ndt = glocal_ndt;
+    glocalndtraw = localndtraw;
+    pthread_mutex_unlock(&mutex);
+
+    gbNeedUpdate = true;
+
+
+    std::cout<<"change map, index is "<<index<<std::endl;
+}
+
+void LocalMapUpdate(int n)
+{
+    std::cout<<"LocalMap n is "<<n<<std::endl;
+
+    int index;
+    double ftracedis;
+    int ncurindex = -1;
+
+    int i;
+    for(i=0;i<gvector_trace.size();i++)
+    {
+//        UseMap(i);
+//        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    }
+    while(gbLMRun)
+    {
+
+        getmindistrace(index,ftracedis);
+
+        if(g_hasmap == false)
+        {
+
+            if(index >= 0)
+            {
+                if(ftracedis < 30)
+                {
+                    UseMap(index);
+                    ncurindex = index;
+                    g_hasmap = true;
+                }
+            }
+        }
+        else
+        {
+           if(index != ncurindex)
+           {
+                pose posex = CalcPose(gvector_trace[ncurindex].mndtorigin,gcurndtgpspos);
+               double fnowdis = gettracedis(ncurindex,posex);
+               if((fnowdis - ftracedis)>5)
+               {
+                   UseMap(index);
+                   ncurindex = index;
+                   g_hasmap = true;
+               }
+           }
+        }
+
+        if(ftracedis > 50)
+        {
+            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" trace dis is "<<ftracedis<<std::endl;
+            std::cout<<" Out Range."<<std::endl;
+            g_hasmap = false;
+        }
+
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+}
+
+
+void SaveMonitor(bool * pbRun)
+{
+    iv::gpspos xoldgpspos;
+    xoldgpspos.lat = 39;
+    xoldgpspos.lon = 117;
+    while(*pbRun)
+    {
+       if(gpFileLastPos != 0)
+       {
+           if(gbFileNDTUpdate)
+           {
+               if((fabs(xoldgpspos.lat - gcurndtgpspos.lat)>0.0000005)||((fabs(xoldgpspos.lon - gcurndtgpspos.lon)>0.0000005)))
+               {
+                   xoldgpspos = gcurndtgpspos;
+                   char str[1000];
+                   snprintf(str,1000,"%11.7f\t%11.7f\t%9.3f\t%6.2f\t%6.2f\t%6.2f\n                ",
+                            xoldgpspos.lon,xoldgpspos.lat,xoldgpspos.height,
+                            xoldgpspos.heading,xoldgpspos.pitch,xoldgpspos.roll);
+                   gpFileLastPos->seek(0);
+                   gpFileLastPos->write(str,strnlen(str,1000));
+                   gpFileLastPos->flush();
+               }
+               gbFileNDTUpdate = false;
+           }
+       }
+       std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+}
+
+void GPSPosMonitor(bool * pbRun)
+{
+    if(gbGPSFix == false)
+    {
+        gcurndtgpspos =  glastndtgpspos;
+    }
+    while(*pbRun)
+    {
+        int64_t nmsnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+        if(abs(nmsnow - gnLastGPSTime)>1000)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            continue;
+        }
+        if(gbGPSFix == true)
+        {
+            double x0,y0;
+            double x,y;
+            GaussProjCal(gcurndtgpspos.lon,gcurndtgpspos.lat,&x0,&y0);
+            GaussProjCal(gcurgpsgpspos.lon,gcurgpsgpspos.lat,&x,&y);
+            double dis = sqrt(pow(x-x0,2)+ pow(y-y0,2));
+            double headdiff = fabs(gcurgpsgpspos.heading - gcurndtgpspos.heading);
+            double zdiff = fabs(gcurgpsgpspos.height - gcurndtgpspos.height);
+            if((dis> 10)||((headdiff>10)&&(headdiff<350))||(zdiff>5)||(g_hasmap == false)||(gbNeedGPSUpdateNDT))
+            {
+                givlog->info("NDTFIX","gps fix ndt pos. dis:%f headdiff:%f zdiff:%f map %d ndt %d",
+                             dis,headdiff,zdiff,g_hasmap,gbNeedGPSUpdateNDT);
+//                gcurndtgpspos = gcurgpsgpspos;
+                  gbgpsupdatendt = true;
+//                current_velocity_x = 0;
+//                current_velocity_y = 0;
+//                current_velocity_z = 0;
+//                angular_velocity = 0;
+//                gbNeedGPSUpdateNDT = false;
+                if(g_hasmap == true)
+                {
+                    int index = gcurmapindex;
+                    if((index >=0)&&(index < gvector_trace.size()))
+                    {
+
+                    }
+                }
+            }
+//            else
+//            {
+//                if(gbgpsupdatendt)
+//                {
+//                    gcurndtgpspos = gcurgpsgpspos;
+//                    gbgpsupdatendt = true;
+//                    current_velocity_x = 0;
+//                    current_velocity_y = 0;
+//                    current_velocity_z = 0;
+//                    angular_velocity = 0;
+//                }
+//            }
+
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+}
+
+
+void ndt_match_Init(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd)
+{
+    _tf_x = 0;
+    _tf_y = 0;
+    _tf_z = 0;
+    _tf_roll = 0;
+    _tf_pitch = 0;
+    _tf_yaw = 0;
+
+    Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z);                 // tl: translation
+    Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX());  // rot: rotation
+    Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
+    tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
+
+
+
+    init_pos_set = 1;
+
+    initial_pose.x = 0;
+    initial_pose.y = 0;
+    initial_pose.z = 0;
+    initial_pose.roll = 0;
+    initial_pose.pitch = 0;
+    initial_pose.yaw = 0;
+
+    localizer_pose.x = initial_pose.x;
+    localizer_pose.y = initial_pose.y;
+    localizer_pose.z = initial_pose.z;
+    localizer_pose.roll = initial_pose.roll;
+    localizer_pose.pitch = initial_pose.pitch;
+    localizer_pose.yaw = initial_pose.yaw;
+
+    previous_pose.x = initial_pose.x;
+    previous_pose.y = initial_pose.y;
+    previous_pose.z = initial_pose.z;
+    previous_pose.roll = initial_pose.roll;
+    previous_pose.pitch = initial_pose.pitch;
+    previous_pose.yaw = initial_pose.yaw;
+
+    current_pose.x = initial_pose.x;
+    current_pose.y = initial_pose.y;
+    current_pose.z = initial_pose.z;
+    current_pose.roll = initial_pose.roll;
+    current_pose.pitch = initial_pose.pitch;
+    current_pose.yaw = initial_pose.yaw;
+
+    current_velocity = 0;
+    current_velocity_x = 0;
+    current_velocity_y = 0;
+    current_velocity_z = 0;
+    angular_velocity = 0;
+
+
+
+
+//    ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
+
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*mappcd));
+
+
+    std::cout<<"map size is"<<mappcd->size()<<std::endl;
+    std::cout<<"ptr size is "<<map_ptr->size()<<std::endl;;
+
+    global_map_ptr = map_ptr;
+
+ //   SetLocalMap();
+
+
+//    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan(new pcl::PointCloud<pcl::PointXYZ>());
+
+//    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
+//    voxel_grid_filter.setLeafSize(1.0,1.0,1.0);
+//    voxel_grid_filter.setInputCloud(map_ptr);
+//    voxel_grid_filter.filter(*filtered_scan);
+
+//    std::cout<<"filter map  size is "<<filtered_scan->size()<<std::endl;;
+
+ //   ndt.setInputTarget(map_ptr);
+//    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+//    ndt.align(*output_cloud, Eigen::Matrix4f::Identity());
+
+
+    gpset = iv::modulecomm::RegisterRecv("ndtposeset",ListenPoseSet);
+    gppose = iv::modulecomm::RegisterSend("ndtpose",10000,3);
+
+    if(map_ptr->size() == 0)
+    {
+        gbNDTRun = false;
+        return;
+    }
+
+
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
+    pcl::PointXYZ dummy_point;
+    dummy_scan_ptr->push_back(dummy_point);
+
+
+
+
+
+    if(map_ptr->size()>0)map_loaded = 1;
+
+ //   gpmapthread = new std::thread(LocalMapUpdate,1);
+
+
+
+
+}
+
+void ndt_match_Init_nomap()
+{
+    _tf_x = 0;
+    _tf_y = 0;
+    _tf_z = 0;
+    _tf_roll = 0;
+    _tf_pitch = 0;
+    _tf_yaw = 0;
+
+    Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z);                 // tl: translation
+    Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX());  // rot: rotation
+    Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
+    tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
+
+
+
+    init_pos_set = 1;
+
+
+
+
+    initial_pose.x = 0;
+    initial_pose.y = 0;
+    initial_pose.z = 0;
+    initial_pose.roll = 0;
+    initial_pose.pitch = 0;
+    initial_pose.yaw = 0;
+
+    localizer_pose.x = initial_pose.x;
+    localizer_pose.y = initial_pose.y;
+    localizer_pose.z = initial_pose.z;
+    localizer_pose.roll = initial_pose.roll;
+    localizer_pose.pitch = initial_pose.pitch;
+    localizer_pose.yaw = initial_pose.yaw;
+
+    previous_pose.x = initial_pose.x;
+    previous_pose.y = initial_pose.y;
+    previous_pose.z = initial_pose.z;
+    previous_pose.roll = initial_pose.roll;
+    previous_pose.pitch = initial_pose.pitch;
+    previous_pose.yaw = initial_pose.yaw;
+
+    current_pose.x = initial_pose.x;
+    current_pose.y = initial_pose.y;
+    current_pose.z = initial_pose.z;
+    current_pose.roll = initial_pose.roll;
+    current_pose.pitch = initial_pose.pitch;
+    current_pose.yaw = initial_pose.yaw;
+
+    current_velocity = 0;
+    current_velocity_x = 0;
+    current_velocity_y = 0;
+    current_velocity_z = 0;
+    angular_velocity = 0;
+
+
+    gpmapthread = new std::thread(LocalMapUpdate,1);
+    gbGFRun = true;
+    gpgpsfixthread = new std::thread(GPSPosMonitor,&gbGFRun);
+    gpsavelastthread = new std::thread(SaveMonitor,&gbGFRun);
+
+
+
+
+}
+
+
+void ndt_match_SetMap(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd)
+{
+    if(mappcd->size() == 0 )return;
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZ>(*mappcd));
+
+
+
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
+    pcl::PointXYZ dummy_point;
+    dummy_scan_ptr->push_back(dummy_point);
+
+
+
+
+
+
+
+    if(map_ptr->size()>0)map_loaded = 1;
+}
+
+
+
+static double wrapToPm(double a_num, const double a_max)
+{
+  if (a_num >= a_max)
+  {
+    a_num -= 2.0 * a_max;
+  }
+  return a_num;
+}
+
+static double wrapToPmPi(const double a_angle_rad)
+{
+  return wrapToPm(a_angle_rad, M_PI);
+}
+
+static double calcDiffForRadian(const double lhs_rad, const double rhs_rad)
+{
+  double diff_rad = lhs_rad - rhs_rad;
+  if (diff_rad >= M_PI)
+    diff_rad = diff_rad - 2 * M_PI;
+  else if (diff_rad < -M_PI)
+    diff_rad = diff_rad + 2 * M_PI;
+  return diff_rad;
+}
+
+
+static unsigned int g_seq_old = 0;
+
+#include <QTime>
+
+
+
+bool isfirst = true;
+
+#include <QMutex>
+std::vector<iv::imudata> gvectorimu;
+QMutex gMuteximu;
+
+static void lidar_imu_calc(qint64 current_lidar_time)
+{
+    int nsize;
+
+    int i;
+    gMuteximu.lock();
+    nsize = gvectorimu.size();
+    for(i=0;i<nsize;i++)
+    {
+        iv::imudata ximudata = gvectorimu[i];
+        double ximu_angular_velocity_x,ximu_angular_velocity_y,ximu_angular_velocity_z;
+        double ximu_linear_acceleration_x,ximu_linear_acceleration_y,ximu_linear_acceleration_z;
+        if(current_lidar_time >  ximudata.imutime)
+        {
+            ximu_angular_velocity_x = ximudata.imu_angular_velocity_x;
+            ximu_angular_velocity_y = ximudata.imu_angular_velocity_y;
+            ximu_angular_velocity_z = ximudata.imu_angular_velocity_z;
+            ximu_linear_acceleration_x = ximudata.imu_linear_acceleration_x;
+            ximu_linear_acceleration_y = ximudata.imu_linear_acceleration_y;
+            ximu_linear_acceleration_z = ximudata.imu_linear_acceleration_z;
+            qint64 current_time_imu = ximudata.imutime;
+//            givlog->verbose("NDT","imu time is %ld",current_time_imu);
+            static qint64 previous_time_imu = current_time_imu;
+            double diff_time = (current_time_imu - previous_time_imu);
+            diff_time = diff_time/1000.0;
+
+            if(diff_time < 0)diff_time = 0.000001;
+            if(diff_time > 0.1)diff_time = 0.1;
+
+            if(current_time_imu < previous_time_imu)
+            {
+
+                std::cout<<"current time is old "<<current_time_imu<<std::endl;
+                previous_time_imu = current_time_imu;
+                continue;
+            }
+
+            double diff_imu_roll = ximu_angular_velocity_x * diff_time;
+            double diff_imu_pitch = ximu_angular_velocity_y * diff_time;
+            double diff_imu_yaw = ximu_angular_velocity_z * diff_time;
+
+            current_pose_imu.roll += diff_imu_roll;
+            current_pose_imu.pitch += diff_imu_pitch;
+            current_pose_imu.yaw += diff_imu_yaw;
+
+            double accX1 = ximu_linear_acceleration_x;
+            double accY1 = std::cos(current_pose_imu.roll) * ximu_linear_acceleration_y -
+                           std::sin(current_pose_imu.roll) * ximu_linear_acceleration_z;
+            double accZ1 = std::sin(current_pose_imu.roll) * ximu_linear_acceleration_y +
+                           std::cos(current_pose_imu.roll) * ximu_linear_acceleration_z;
+
+            double accX2 = std::sin(current_pose_imu.pitch) * accZ1 + std::cos(current_pose_imu.pitch) * accX1;
+            double accY2 = accY1;
+            double accZ2 = std::cos(current_pose_imu.pitch) * accZ1 - std::sin(current_pose_imu.pitch) * accX1;
+
+            double accX = std::cos(current_pose_imu.yaw) * accX2 - std::sin(current_pose_imu.yaw) * accY2;
+            double accY = std::sin(current_pose_imu.yaw) * accX2 + std::cos(current_pose_imu.yaw) * accY2;
+            double accZ = accZ2;
+
+            offset_imu_x += current_velocity_imu_x * diff_time + accX * diff_time * diff_time / 2.0;
+            offset_imu_y += current_velocity_imu_y * diff_time + accY * diff_time * diff_time / 2.0;
+            offset_imu_z += current_velocity_imu_z * diff_time + accZ * diff_time * diff_time / 2.0;
+
+            current_velocity_imu_x += accX * diff_time;
+            current_velocity_imu_y += accY * diff_time;
+            current_velocity_imu_z += accZ * diff_time;
+
+            offset_imu_roll += diff_imu_roll;
+            offset_imu_pitch += diff_imu_pitch;
+            offset_imu_yaw += diff_imu_yaw;
+
+          //  guess_pose_imu.x = previous_pose.x + offset_imu_x;
+          //  guess_pose_imu.y = previous_pose.y + offset_imu_y;
+          //  guess_pose_imu.z = previous_pose.z + offset_imu_z;
+          //  guess_pose_imu.roll = previous_pose.roll + offset_imu_roll;
+          //  guess_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
+          //  guess_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
+
+            predict_pose_imu.x = previous_pose.x + offset_imu_x;
+            predict_pose_imu.y = previous_pose.y + offset_imu_y;
+            predict_pose_imu.z = previous_pose.z + offset_imu_z;
+            predict_pose_imu.roll = previous_pose.roll + offset_imu_roll;
+            predict_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
+            predict_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
+
+            givlog->verbose("NDT","imu x:%f y:%f z:%f yaw:%f",offset_imu_x,
+                            offset_imu_y,offset_imu_z,offset_imu_yaw);
+
+            previous_time_imu = current_time_imu;
+
+
+        }
+        else
+        {
+            break;;
+        }
+
+    }
+
+    if(i>0)
+    {
+        gvectorimu.erase(gvectorimu.begin(),gvectorimu.begin()+i);
+    }
+
+    gMuteximu.unlock();
+
+//    for(i=0;i<gvectorimu.size();i++)
+//    {
+//        iv::imudata ximudata = gvectorimu[i];
+//        double ximu_angular_velocity_x,ximu_angular_velocity_y,ximu_angular_velocity_z;
+//        double ximu_linear_acceleration_x,ximu_linear_acceleration_y,ximu_linear_acceleration_z;
+//        if(current_lidar_time >  ximudata.imutime)
+//        {
+
+//            gvectorimu.erase(gvectorimu.begin())
+//        }
+//    }
+}
+
+
+static void imu_calc(qint64 current_time_imu)
+{
+
+  static qint64 previous_time_imu = current_time_imu;
+  double diff_time = (current_time_imu - previous_time_imu);
+  diff_time = diff_time/1000.0;
+
+  if(current_time_imu < previous_time_imu)
+  {
+      std::cout<<"current time is old "<<current_time_imu<<std::endl;
+      return;
+  }
+
+  double diff_imu_roll = imu_angular_velocity_x * diff_time;
+  double diff_imu_pitch = imu_angular_velocity_y * diff_time;
+  double diff_imu_yaw = imu_angular_velocity_z * diff_time;
+
+  current_pose_imu.roll += diff_imu_roll;
+  current_pose_imu.pitch += diff_imu_pitch;
+  current_pose_imu.yaw += diff_imu_yaw;
+
+  double accX1 = imu_linear_acceleration_x;
+  double accY1 = std::cos(current_pose_imu.roll) * imu_linear_acceleration_y -
+                 std::sin(current_pose_imu.roll) * imu_linear_acceleration_z;
+  double accZ1 = std::sin(current_pose_imu.roll) * imu_linear_acceleration_y +
+                 std::cos(current_pose_imu.roll) * imu_linear_acceleration_z;
+
+  double accX2 = std::sin(current_pose_imu.pitch) * accZ1 + std::cos(current_pose_imu.pitch) * accX1;
+  double accY2 = accY1;
+  double accZ2 = std::cos(current_pose_imu.pitch) * accZ1 - std::sin(current_pose_imu.pitch) * accX1;
+
+  double accX = std::cos(current_pose_imu.yaw) * accX2 - std::sin(current_pose_imu.yaw) * accY2;
+  double accY = std::sin(current_pose_imu.yaw) * accX2 + std::cos(current_pose_imu.yaw) * accY2;
+  double accZ = accZ2;
+
+  offset_imu_x += current_velocity_imu_x * diff_time + accX * diff_time * diff_time / 2.0;
+  offset_imu_y += current_velocity_imu_y * diff_time + accY * diff_time * diff_time / 2.0;
+  offset_imu_z += current_velocity_imu_z * diff_time + accZ * diff_time * diff_time / 2.0;
+
+  current_velocity_imu_x += accX * diff_time;
+  current_velocity_imu_y += accY * diff_time;
+  current_velocity_imu_z += accZ * diff_time;
+
+  offset_imu_roll += diff_imu_roll;
+  offset_imu_pitch += diff_imu_pitch;
+  offset_imu_yaw += diff_imu_yaw;
+
+//  guess_pose_imu.x = previous_pose.x + offset_imu_x;
+//  guess_pose_imu.y = previous_pose.y + offset_imu_y;
+//  guess_pose_imu.z = previous_pose.z + offset_imu_z;
+//  guess_pose_imu.roll = previous_pose.roll + offset_imu_roll;
+//  guess_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
+//  guess_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
+
+  predict_pose_imu.x = previous_pose.x + offset_imu_x;
+  predict_pose_imu.y = previous_pose.y + offset_imu_y;
+  predict_pose_imu.z = previous_pose.z + offset_imu_z;
+  predict_pose_imu.roll = previous_pose.roll + offset_imu_roll;
+  predict_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
+  predict_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
+
+  givlog->verbose("NDT","imu x:%f y:%f z:%f yaw:%f",offset_imu_x,
+                  offset_imu_y,offset_imu_z,offset_imu_yaw);
+
+  previous_time_imu = current_time_imu;
+}
+
+
+void imu_update(qint64 current_time_imu,double imu_roll,double imu_pitch,double imu_yaw,
+                       double acceleration_x,double acceleration_y,
+                       double acceleration_z)
+{
+  // std::cout << __func__ << std::endl;
+
+// double imu_angular_velocity_x,imu_angular_velocity_y,imu_angular_velocity_z;
+
+  static double previous_time_imu = current_time_imu;
+  double diff_time = (current_time_imu - previous_time_imu);
+  diff_time = diff_time/1000.0;
+
+
+  imu_roll = imu_roll * M_PI/180.0;
+  imu_pitch = imu_pitch * M_PI/180.0;
+  imu_yaw = (-1.0)*imu_yaw * M_PI/180.0;
+
+  imu_yaw = imu_yaw + 2.0*M_PI;
+
+//  imu_pitch = 0;
+//  imu_roll = 0;
+
+  imu_roll = wrapToPmPi(imu_roll);
+  imu_pitch = wrapToPmPi(imu_pitch);
+  imu_yaw = wrapToPmPi(imu_yaw);
+
+  static double previous_imu_roll = imu_roll, previous_imu_pitch = imu_pitch, previous_imu_yaw = imu_yaw;
+  const double diff_imu_roll = imu_roll - previous_imu_roll;
+
+  const double diff_imu_pitch = imu_pitch - previous_imu_pitch;
+
+  double diff_imu_yaw;
+  if (fabs(imu_yaw - previous_imu_yaw) > M_PI)
+  {
+    if (imu_yaw > 0)
+      diff_imu_yaw = (imu_yaw - previous_imu_yaw) - M_PI * 2;
+    else
+      diff_imu_yaw = -M_PI * 2 - (imu_yaw - previous_imu_yaw);
+  }
+  else
+    diff_imu_yaw = imu_yaw - previous_imu_yaw;
+
+
+  imu_linear_acceleration_x = acceleration_x;
+//  imu_linear_acceleration_y = acceleration_y;
+//  imu_linear_acceleration_z = acceleration_z;
+  imu_linear_acceleration_y = 0;
+  imu_linear_acceleration_z = 0;
+
+  if (diff_time != 0)
+  {
+    imu_angular_velocity_x = diff_imu_roll / diff_time;
+    imu_angular_velocity_y = diff_imu_pitch / diff_time;
+    imu_angular_velocity_z = diff_imu_yaw / diff_time;
+  }
+  else
+  {
+    imu_angular_velocity_x = 0;
+    imu_angular_velocity_y = 0;
+    imu_angular_velocity_z = 0;
+  }
+
+  iv::imudata ximudata;
+  ximudata.imutime = current_time_imu;
+  ximudata.imu_linear_acceleration_x = imu_linear_acceleration_x;
+  ximudata.imu_linear_acceleration_y = imu_linear_acceleration_y;
+  ximudata.imu_linear_acceleration_z = imu_linear_acceleration_z;
+  ximudata.imu_angular_velocity_x = imu_angular_velocity_x;
+  ximudata.imu_angular_velocity_y = imu_angular_velocity_y;
+  ximudata.imu_angular_velocity_z = imu_angular_velocity_z;
+
+  gMuteximu.lock();
+  gvectorimu.push_back(ximudata);
+  gMuteximu.unlock();
+//  imu_calc(current_time_imu);
+
+  previous_time_imu = current_time_imu;
+  previous_imu_roll = imu_roll;
+  previous_imu_pitch = imu_pitch;
+  previous_imu_yaw = imu_yaw;
+}
+
+
+#ifdef TEST_CALCTIME
+int ncalc = 0;
+int gncalctotal = 0;
+#endif
+void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
+{
+
+    static bool bNeedUpdateVel = false; //if gps update ndt, need update velocity
+    qint64 current_scan_time = raw_scan->header.stamp;
+    static qint64 old_scan_time = current_scan_time;
+    if(gbNDTRun == false)return;
+
+    bool bNotChangev = false;
+
+    if(gbgpsupdatendt == true)
+    {
+
+        std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" update ndt pos use gps pos. "<<std::endl;
+        gcurndtgpspos = gcurgpsgpspos;
+        gbgpsupdatendt = false;
+        gbNeedGPSUpdateNDT = false;
+        current_velocity_x = 0;
+        current_velocity_y = 0;
+        current_velocity_z = 0;
+        angular_velocity = 0;
+        bNeedUpdateVel = true;
+
+//
+//
+//        gcurndtgpspos = gcurgpsgpspos;
+//        current_pose = CalcPose(gvector_trace[gusemapindex].mndtorigin,gcurndtgpspos);
+//       predict_pose_for_ndt = current_pose;
+        current_velocity_imu_x = 0;
+        current_velocity_imu_y = 0;
+        current_velocity_imu_z = 0;
+        gMuteximu.lock();
+        gvectorimu.erase(gvectorimu.begin(),gvectorimu.begin() + gvectorimu.size());
+        gMuteximu.unlock();
+        bNotChangev = true;
+        return;
+    }
+//    gbgpsupdatendt = false;
+
+    if(g_hasmap == false)
+    {
+        return;
+    }
+    if(gbNeedUpdate)
+    {
+        pthread_mutex_lock(&mutex);
+        ndtraw = glocalndtraw;
+        pthread_mutex_unlock(&mutex);
+        gusemapindex = gcurmapindex;
+        gbNeedUpdate = false;
+
+    }
+
+
+
+
+
+    previous_pose = CalcPose(gvector_trace[gusemapindex].mndtorigin,gcurndtgpspos);
+    if(bNeedUpdateVel == true)
+    {
+        bNeedUpdateVel = false;
+        current_velocity_x = previous_pose.vx;
+        current_velocity_y = previous_pose.vy;
+        current_velocity_imu_x = current_velocity_x;
+        current_velocity_imu_y = current_velocity_y;
+    }
+    givlog->verbose("previos pose is %f %f",previous_pose.x,previous_pose.y);
+
+//    if(map_loaded == 0)
+//    {
+//        std::cout<<BOLDRED<<" point_ndt not set map."<<RESET<<std::endl;
+//        return;
+//    }
+
+
+
+    pthread_mutex_lock(&mutex_pose);
+
+    QTime xTime;
+    xTime.start();
+//    std::cout<<"time is "<<xTime.elapsed()<<std::endl;
+    double voxel_leaf_size = 2.0;
+    double measurement_range = 200.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);
+
+//    std::cout<<"voxed time is "<<xTime.elapsed()<<std::endl;
+
+//    std::cout<<" seq is "<<raw_scan->header.seq<<std::endl;
+  //  qDebug("seq = %d stamp = %d ",raw_scan->header.seq,raw_scan->header.stamp);
+  //  std::cout<<"raw scan size is "<<raw_scan->size()<<"  filtered scan size is "<<filtered_scan->size()<<std::endl;
+
+ //   return;
+    matching_start = std::chrono::system_clock::now();
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(*filtered_scan));
+
+    int scan_points_num = filtered_scan_ptr->size();
+
+    Eigen::Matrix4f t(Eigen::Matrix4f::Identity());   // base_link
+    Eigen::Matrix4f t2(Eigen::Matrix4f::Identity());  // localizer
+
+    std::chrono::time_point<std::chrono::system_clock> align_start, align_end, getFitnessScore_start,
+        getFitnessScore_end;
+    static double align_time, getFitnessScore_time = 0.0;
+
+//    std::cout<<"run hear"<<std::endl;
+    pthread_mutex_lock(&mutex);
+
+//    if (_method_type == MethodType::PCL_GENERIC)
+ //   ndt.setInputSource(filtered_scan_ptr);
+
+
+    ndtraw->setInputSource(filtered_scan_ptr);
+
+    // Guess the initial gross estimation of the transformation
+//    double diff_time = (current_scan_time - previous_scan_time).toSec();
+
+
+    double diff_time = 0.0;
+
+    if(g_seq_old != 0)
+    {
+        if((raw_scan->header.seq - g_seq_old)>0)
+        {
+            diff_time = raw_scan->header.seq - g_seq_old;
+            diff_time = diff_time * 0.1;
+        }
+    }
+
+    g_seq_old = raw_scan->header.seq;
+
+    diff_time = current_scan_time -old_scan_time;
+    diff_time = diff_time/1000.0;
+    old_scan_time = current_scan_time;
+
+    if(diff_time<=0)diff_time = 0.1;
+    if(diff_time>1.0)diff_time = 0.1;
+
+//    std::cout<<"diff time is "<<diff_time<<std::endl;
+
+//    std::cout<<" diff time is "<<diff_time<<std::endl;
+
+//    diff_time = 0.0;
+
+//    if (_offset == "linear")
+//    {
+
+      if(diff_time<0.1)diff_time = 0.1;
+      offset_x = current_velocity_x * diff_time;
+      offset_y = current_velocity_y * diff_time;
+      offset_z = current_velocity_z * diff_time;
+      offset_yaw = angular_velocity * diff_time;
+//    }
+      predict_pose.x = previous_pose.x + offset_x;
+      predict_pose.y = previous_pose.y + offset_y;
+      predict_pose.z = previous_pose.z + offset_z;
+      predict_pose.roll = previous_pose.roll;
+      predict_pose.pitch = previous_pose.pitch;
+      predict_pose.yaw = previous_pose.yaw + offset_yaw;
+
+      pose predict_pose_for_ndt;
+
+
+      givlog->verbose("NDT","previous x:%f y:%f z:%f yaw:%f",previous_pose.x,
+                      previous_pose.y,previous_pose.z,previous_pose.yaw);
+//      if (_use_imu == true && _use_odom == true)
+//         imu_odom_calc(current_scan_time);
+       if (_use_imu == true && _use_odom == false)
+       {
+         lidar_imu_calc((current_scan_time-0));
+       }
+//       if (_use_imu == false && _use_odom == true)
+//         odom_calc(current_scan_time);
+
+//       if (_use_imu == true && _use_odom == true)
+//         predict_pose_for_ndt = predict_pose_imu_odom;
+//       else
+//           if (_use_imu == true && _use_odom == false)
+//         predict_pose_for_ndt = predict_pose_imu;
+//       else if (_use_imu == false && _use_odom == true)
+//         predict_pose_for_ndt = predict_pose_odom;
+//       else
+//         predict_pose_for_ndt = predict_pose;
+
+      if (_use_imu == true && _use_odom == false)
+      {
+         predict_pose_for_ndt = predict_pose_imu;
+ //         predict_pose_for_ndt = predict_pose;
+ //         predict_pose_for_ndt.yaw = predict_pose_imu.yaw;
+      }
+      else
+      {
+          predict_pose_for_ndt = predict_pose;
+      }
+
+      predict_pose_for_ndt = predict_pose; //not use predict_pose_imu in shelan.
+
+      if(fabs(predict_pose_for_ndt.x -previous_pose.x)>10)
+      {
+          predict_pose_for_ndt = previous_pose;
+      }
+
+      if(fabs(predict_pose_for_ndt.y -previous_pose.y)>10)
+      {
+          predict_pose_for_ndt = previous_pose;
+      }
+
+ //     predict_pose_for_ndt = predict_pose;
+
+
+
+
+      givlog->verbose("NDT","pre x:%f y:%f z:%f yaw:%f",predict_pose_for_ndt.x,
+                      predict_pose_for_ndt.y,predict_pose_for_ndt.z,predict_pose_for_ndt.yaw);
+//      predict_pose_for_ndt = predict_pose;
+
+      Eigen::Translation3f init_translation(predict_pose_for_ndt.x, predict_pose_for_ndt.y, predict_pose_for_ndt.z);
+      Eigen::AngleAxisf init_rotation_x(predict_pose_for_ndt.roll, Eigen::Vector3f::UnitX());
+      Eigen::AngleAxisf init_rotation_y(predict_pose_for_ndt.pitch, Eigen::Vector3f::UnitY());
+      Eigen::AngleAxisf init_rotation_z(predict_pose_for_ndt.yaw, 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 output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+ //     std::cout<<"before ndt time is "<<xTime.elapsed()<<std::endl;
+//      std::cout<<"time is "<<xTime.elapsed()<<std::endl;
+      pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
+      align_start = std::chrono::system_clock::now();
+      ndtraw->align(*aligned,init_guess);
+      align_end = std::chrono::system_clock::now();
+
+      if(xTime.elapsed()<90)
+      std::cout<<GREEN<<"after ndt time is "<<xTime.elapsed()<<RESET<<std::endl;
+      else
+          std::cout<<RED<<"after ndt time is "<<xTime.elapsed()<<RESET<<std::endl;
+
+
+#ifdef TEST_CALCTIME
+
+    gncalctotal = gncalctotal + xTime.elapsed();
+    ncalc++;
+    if(ncalc>0)
+    {
+        std::cout<<" average calc time: "<<gncalctotal/ncalc<<std::endl;
+
+    }
+#endif
+
+      has_converged = ndtraw->hasConverged();
+
+      t = ndtraw->getFinalTransformation();
+      iteration = ndtraw->getFinalNumIteration();
+
+      getFitnessScore_start = std::chrono::system_clock::now();
+      fitness_score = ndtraw->getFitnessScore();
+      getFitnessScore_end = std::chrono::system_clock::now();
+
+      trans_probability = ndtraw->getTransformationProbability();
+
+      std::cout<<"score: "<<fitness_score<<" trans_probability:"<<trans_probability<<std::endl;
+  //    std::cout<<" scoure is "<<fitness_score<<std::endl;
+
+//      std::cout<<" iter is "<<iteration<<std::endl;
+
+      t2 = t * tf_btol.inverse();
+
+      getFitnessScore_time =
+          std::chrono::duration_cast<std::chrono::microseconds>(getFitnessScore_end - getFitnessScore_start).count() /
+          1000.0;
+
+      pthread_mutex_unlock(&mutex);
+
+      tf::Matrix3x3 mat_l;  // localizer
+      mat_l.setValue(static_cast<double>(t(0, 0)), static_cast<double>(t(0, 1)), static_cast<double>(t(0, 2)),
+                     static_cast<double>(t(1, 0)), static_cast<double>(t(1, 1)), static_cast<double>(t(1, 2)),
+                     static_cast<double>(t(2, 0)), static_cast<double>(t(2, 1)), static_cast<double>(t(2, 2)));
+
+      // Update localizer_pose
+      localizer_pose.x = t(0, 3);
+      localizer_pose.y = t(1, 3);
+      localizer_pose.z = t(2, 3);
+      mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1);
+
+
+//     std::cout<<" local pose x is "<<localizer_pose.x<<std::endl;
+
+      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
+      ndt_pose.x = t2(0, 3);
+      ndt_pose.y = t2(1, 3);
+      ndt_pose.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(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw, 1);
+
+
+      givlog->verbose("NDT","calc x:%f y:%f z:%f yaw:%f",ndt_pose.x,
+                      ndt_pose.y,ndt_pose.z,ndt_pose.yaw);
+
+
+      // Calculate the difference between ndt_pose and predict_pose
+      predict_pose_error = sqrt((ndt_pose.x - predict_pose_for_ndt.x) * (ndt_pose.x - predict_pose_for_ndt.x) +
+                                (ndt_pose.y - predict_pose_for_ndt.y) * (ndt_pose.y - predict_pose_for_ndt.y) +
+                                (ndt_pose.z - predict_pose_for_ndt.z) * (ndt_pose.z - predict_pose_for_ndt.z));
+
+
+ //     std::cout<<" pose error is "<<predict_pose_error<<std::endl;
+ //     std::cout<<"ndt pose x is "<<ndt_pose.x<<std::endl;
+      if (predict_pose_error <= (PREDICT_POSE_THRESHOLD*10*diff_time))
+      {
+        use_predict_pose = 0;
+      }
+      else
+      {
+        use_predict_pose = 1;
+      }
+      use_predict_pose = 0;
+
+      if (use_predict_pose == 0)
+      {
+        current_pose.x = ndt_pose.x;
+        current_pose.y = ndt_pose.y;
+        current_pose.z = ndt_pose.z;
+        current_pose.roll = ndt_pose.roll;
+        current_pose.pitch = ndt_pose.pitch;
+        current_pose.yaw = ndt_pose.yaw;
+
+      }
+      else
+      {
+          givlog->verbose("NDT","USE PREDICT POSE");
+        current_pose.x = predict_pose_for_ndt.x;
+        current_pose.y = predict_pose_for_ndt.y;
+        current_pose.z = predict_pose_for_ndt.z;
+        current_pose.roll = predict_pose_for_ndt.roll;
+        current_pose.pitch = predict_pose_for_ndt.pitch;
+        current_pose.yaw = predict_pose_for_ndt.yaw;
+      }
+
+ //     std::cout<<" current pose x is "<<current_pose.x<<std::endl;
+
+      // Compute the velocity and acceleration
+      diff_x = current_pose.x - previous_pose.x;
+      diff_y = current_pose.y - previous_pose.y;
+      diff_z = current_pose.z - previous_pose.z;
+      diff_yaw = calcDiffForRadian(current_pose.yaw, previous_pose.yaw);
+      diff = sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z);
+
+      current_velocity = (diff_time > 0) ? (diff / diff_time) : 0;
+      current_velocity_x = (diff_time > 0) ? (diff_x / diff_time) : 0;
+      current_velocity_y = (diff_time > 0) ? (diff_y / diff_time) : 0;
+      current_velocity_z = (diff_time > 0) ? (diff_z / diff_time) : 0;
+      angular_velocity = (diff_time > 0) ? (diff_yaw / diff_time) : 0;
+
+      current_pose.vx = current_velocity_x;
+      current_pose.vy = current_velocity_y;
+
+      current_velocity_smooth = (current_velocity + previous_velocity + previous_previous_velocity) / 3.0;
+      if (current_velocity_smooth < 0.2)
+      {
+        current_velocity_smooth = 0.0;
+      }
+
+ //     bNotChangev = true;
+      if((diff_time >  0)&&(bNotChangev == false))
+      {
+          double aa = (current_velocity -previous_velocity)/diff_time;
+          if(fabs(aa)>5.0)
+          {
+              givlog->verbose("NDT","aa is %f",aa);
+              aa = fabs(5.0/aa);
+              current_velocity = previous_velocity + aa*(current_velocity -previous_velocity);
+              current_velocity_x = previous_velocity_x + aa *((current_velocity_x -previous_velocity_x));
+              current_velocity_y = previous_velocity_y + aa *((current_velocity_y -previous_velocity_y));
+              current_velocity_z = previous_velocity_z + aa *((current_velocity_z -previous_velocity_z));
+
+          }
+      }
+
+      current_accel = (diff_time > 0) ? ((current_velocity - previous_velocity) / diff_time) : 0;
+      current_accel_x = (diff_time > 0) ? ((current_velocity_x - previous_velocity_x) / diff_time) : 0;
+      current_accel_y = (diff_time > 0) ? ((current_velocity_y - previous_velocity_y) / diff_time) : 0;
+      current_accel_z = (diff_time > 0) ? ((current_velocity_z - previous_velocity_z) / diff_time) : 0;
+
+ //   std::cout<<"fit time is "<<getFitnessScore_time<<std::endl;
+
+      gcurndtgpspos = PoseToGPS(gvector_trace[gcurmapindex].mndtorigin,current_pose);
+//      std::cout<<" pre x:"<<previous_pose.x<<" pre y:"<<previous_pose.y<<std::endl;
+    std::cout<<" x: "<<current_pose.x<<" y: "<<current_pose.y
+            <<" z: "<<current_pose.z<<" yaw:"<<current_pose.yaw
+           <<" vel: "<<current_velocity<<std::endl;
+
+
+    std::cout<<"NDT ve:"<<gcurndtgpspos.ve<<" vn:"<<gcurndtgpspos.vn
+            <<" GPS ve:"<<gcurgpsgpspos.ve<<" vn:"<<gcurgpsgpspos.vn<<std::endl;
+
+    gbFileNDTUpdate = true;
+
+//    gw->Updatexyyaw(current_pose.x,current_pose.y,current_pose.yaw);
+
+    current_pose_imu.x = current_pose.x;
+    current_pose_imu.y = current_pose.y;
+    current_pose_imu.z = current_pose.z;
+    current_pose_imu.roll = current_pose.roll;
+    current_pose_imu.pitch = current_pose.pitch;
+    current_pose_imu.yaw = current_pose.yaw;
+
+    current_velocity_imu_x = current_velocity_x;
+    current_velocity_imu_y = current_velocity_y;
+    current_velocity_imu_z = current_velocity_z;
+
+    current_velocity_imu_z = 0;
+
+    offset_imu_x = 0.0;
+    offset_imu_y = 0.0;
+    offset_imu_z = 0.0;
+    offset_imu_roll = 0.0;
+    offset_imu_pitch = 0.0;
+    offset_imu_yaw = 0.0;
+
+//    std::cout<<" vx is "<<current_velocity_x<<" vy is "<<current_velocity_y<<std::endl;
+    previous_pose.x = current_pose.x;
+    previous_pose.y = current_pose.y;
+    previous_pose.z = current_pose.z;
+    previous_pose.roll = current_pose.roll;
+    previous_pose.pitch = current_pose.pitch;
+    previous_pose.yaw = current_pose.yaw;
+
+//    previous_scan_time = current_scan_time;
+
+    previous_previous_velocity = previous_velocity;
+    previous_velocity = current_velocity;
+    previous_velocity_x = current_velocity_x;
+    previous_velocity_y = current_velocity_y;
+    previous_velocity_z = current_velocity_z;
+    previous_accel = current_accel;
+
+    pthread_mutex_lock(&mutex_read);
+    gindex++;
+    glidar_pose.accel = current_accel;
+    glidar_pose.accel_x = current_accel_x;
+    glidar_pose.accel_y = current_accel_y;
+    glidar_pose.accel_z = current_accel_z;
+    glidar_pose.vel = current_velocity;
+    glidar_pose.vel_x = current_velocity_x;
+    glidar_pose.vel_y = current_velocity_y;
+    glidar_pose.vel_z = current_velocity_z;
+    glidar_pose.mpose = current_pose;
+    pthread_mutex_unlock(&mutex_read);
+
+    pthread_mutex_unlock(&mutex_pose);
+
+
+
+
+    double hdg_o = (90 - gvector_trace[gcurmapindex].mndtorigin.heading)*M_PI/180.0;
+    double ndt_vn = current_velocity_x * cos(hdg_o) - current_velocity_y * sin(hdg_o);
+    double ndt_ve = current_velocity_x * sin(hdg_o) + current_velocity_y * cos(hdg_o);
+    double ndt_vd = current_accel_z;
+
+
+
+    std::cout<<std::setprecision(9)<<"gps lat:"<<gcurndtgpspos.lat<<" lon:"
+            <<gcurndtgpspos.lon<<" z:"<<gcurndtgpspos.height<<" heading:"
+           <<gcurndtgpspos.heading<<std::endl;
+    std::cout<<std::setprecision(6)<<std::endl;
+
+
+
+
+    double x0,y0;
+    double x,y;
+    GaussProjCal(gcurndtgpspos.lon,gcurndtgpspos.lat,&x0,&y0);
+    GaussProjCal(gcurgpsgpspos.lon,gcurgpsgpspos.lat,&x,&y);
+    double dis = sqrt(pow(x-x0,2)+ pow(y-y0,2));
+    double headdiff = fabs(gcurgpsgpspos.heading - gcurndtgpspos.heading);
+    double zdiff = fabs(gcurgpsgpspos.height - gcurndtgpspos.height);
+    std::cout<<" dis:"<<dis<<" headdiff:"<<headdiff<<" zdiff:"<<zdiff<<std::endl;
+
+    iv::lidar::ndtpos ndtpos;
+    ndtpos.set_lidartime(raw_scan->header.stamp/1000);
+    ndtpos.set_ndt_time(QDateTime::currentMSecsSinceEpoch());
+    ndtpos.set_accel(current_accel);
+    ndtpos.set_accel_x(current_accel_x);
+    ndtpos.set_accel_y(current_accel_y);
+    ndtpos.set_accel_z(current_accel_z);
+    ndtpos.set_vel(current_velocity);
+    ndtpos.set_vel_x(current_velocity_x);
+    ndtpos.set_vel_y(current_velocity_y);
+    ndtpos.set_vel_z(current_velocity_z);
+    ndtpos.set_fitness_score(fitness_score);
+    ndtpos.set_has_converged(has_converged);
+    ndtpos.set_id(0);
+    ndtpos.set_iteration(iteration);
+    ndtpos.set_trans_probability(trans_probability);
+    ndtpos.set_pose_pitch(current_pose.pitch);
+    ndtpos.set_pose_roll(current_pose.roll);
+    ndtpos.set_pose_yaw(current_pose.yaw);
+    ndtpos.set_pose_x(current_pose.x);
+    ndtpos.set_pose_y(current_pose.y);
+    ndtpos.set_pose_z(current_pose.z);
+    ndtpos.set_comp_time(xTime.elapsed());
+
+
+    int ndatasize = ndtpos.ByteSize();
+    char * strser = new char[ndatasize];
+    bool bSer = ndtpos.SerializeToArray(strser,ndatasize);
+    if(bSer)
+    {
+        iv::modulecomm::ModuleSendMsg(gpc,strser,ndatasize);
+    }
+    else
+    {
+        std::cout<<BOLDRED<<"ndtpos serialize error."<<RESET<<std::endl;
+    }
+    delete strser;
+
+    iv::lidar::ndtgpspos xndtgpspos;
+    xndtgpspos.set_lat(gcurndtgpspos.lat);
+    xndtgpspos.set_lon(gcurndtgpspos.lon);
+    xndtgpspos.set_heading(gcurndtgpspos.heading);
+    xndtgpspos.set_height(gcurndtgpspos.height);
+    xndtgpspos.set_pitch(gcurndtgpspos.pitch);
+    xndtgpspos.set_roll(gcurndtgpspos.roll);
+    xndtgpspos.set_tras_prob(trans_probability);
+    xndtgpspos.set_score(fitness_score);
+    xndtgpspos.set_vn(ndt_vn);
+    xndtgpspos.set_ve(ndt_ve);
+    xndtgpspos.set_vd(ndt_vd);
+
+    givlog->debug("ndtgpspos","lat:%11.7f lon:%11.7f height:%11.3f heading:%6.3f pitch:%6.3f roll:%6.3f vn:%6.3f ve:%6.3f vd:%6.3f trans_prob:%6.3f score:%6.3f",
+                  xndtgpspos.lat(),xndtgpspos.lon(),xndtgpspos.height(),
+                  xndtgpspos.heading(),xndtgpspos.pitch(),xndtgpspos.roll(),
+                  xndtgpspos.vn(),xndtgpspos.ve(),xndtgpspos.vd(),
+                  xndtgpspos.tras_prob(),xndtgpspos.score());
+
+    ndatasize = xndtgpspos.ByteSize();
+    strser = new char[ndatasize];
+    bSer = xndtgpspos.SerializeToArray(strser,ndatasize);
+    if(bSer)
+    {
+        std::cout<<"share msg."<<std::endl;
+        iv::modulecomm::ModuleSendMsg(gpd,strser,ndatasize);
+    }
+    else
+    {
+        std::cout<<BOLDRED<<"ndtgpspos serialize error."<<RESET<<std::endl;
+    }
+    delete strser;
+
+    if(trans_probability < 0.5)gbNeedGPSUpdateNDT = true;
+    else gbNeedGPSUpdateNDT = false;
+
+
+}
+
+
+int GetPose(int & curindex,iv::lidar_pose & xlidar_pose)
+{
+     pthread_mutex_lock(&mutex_read);
+    if(curindex == gindex)
+    {
+        pthread_mutex_unlock(&mutex_read);
+        return 0;
+    }
+    curindex = gindex;
+    xlidar_pose = glidar_pose;
+    pthread_mutex_unlock(&mutex_read);
+    return 1;
+}
+
+void SetCurPose(double x0, double y0, double yaw0, double z0, double pitch0, double roll0)
+{
+
+    std::cout<<"set pose"<<std::endl;
+    pthread_mutex_lock(&mutex_pose);
+
+    initial_pose.x = x0;
+    initial_pose.y = y0;
+    initial_pose.z = z0;
+    initial_pose.roll = roll0;
+    initial_pose.pitch = pitch0;
+    initial_pose.yaw = yaw0;
+
+    localizer_pose.x = initial_pose.x;
+    localizer_pose.y = initial_pose.y;
+    localizer_pose.z = initial_pose.z;
+    localizer_pose.roll = initial_pose.roll;
+    localizer_pose.pitch = initial_pose.pitch;
+    localizer_pose.yaw = initial_pose.yaw;
+
+    previous_pose.x = initial_pose.x;
+    previous_pose.y = initial_pose.y;
+    previous_pose.z = initial_pose.z;
+    previous_pose.roll = initial_pose.roll;
+    previous_pose.pitch = initial_pose.pitch;
+    previous_pose.yaw = initial_pose.yaw;
+
+    current_pose.x = initial_pose.x;
+    current_pose.y = initial_pose.y;
+    current_pose.z = initial_pose.z;
+    current_pose.roll = initial_pose.roll;
+    current_pose.pitch = initial_pose.pitch;
+    current_pose.yaw = initial_pose.yaw;
+
+    current_velocity = 0;
+    current_velocity_x = 0;
+    current_velocity_y = 0;
+    current_velocity_z = 0;
+    angular_velocity = 0;
+
+    pthread_mutex_unlock(&mutex_pose);
+
+
+}
+
+void SetRunState(bool bRun)
+{
+    std::cout<<"set state."<<std::endl;
+    gbNDTRun = bRun;
+}
+
+void setuseimu(bool bUse)
+{
+    _use_imu = bUse;
+}
+

src/detection/detection_ndt_matching/ndt_matching.h

@@ -0,0 +1,112 @@
+#ifndef NDT_MATCHING_H
+#define NDT_MATCHING_H
+#include<QDateTime>
+#include <QMutex>
+
+struct pose
+{
+  double x;
+  double y;
+  double z;
+  double roll;
+  double pitch;
+  double yaw;
+  double vx;
+  double vy;
+};
+
+
+namespace  iv {
+struct lidar_pose
+{
+    lidar_pose() {}
+
+    double vel_x;
+    double vel_y;
+    double vel_z;
+    double vel;
+    double accel_x;
+    double accel_y;
+    double accel_z;
+    double accel;
+    struct pose mpose;
+};
+
+}
+
+namespace iv {
+struct imudata
+{
+  qint64 imutime;
+  double imu_linear_acceleration_x;
+  double imu_linear_acceleration_y;
+  double imu_linear_acceleration_z;
+  double imu_angular_velocity_x;
+  double imu_angular_velocity_y;
+  double imu_angular_velocity_z;
+};
+}
+
+
+void ndt_match_Init_nomap();
+
+void ndt_match_Init(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd);
+
+void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan);
+
+void point_ndt_test(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan);
+
+int GetPose(int & curindex,iv::lidar_pose & xlidar_pose);
+
+void SetCurPose(double x0,double y0,double yaw0,double z0 = 0,double pitch0 = 0,double roll0 = 0);
+
+void ndt_match_SetMap(pcl::PointCloud<pcl::PointXYZ>::Ptr mappcd);
+
+void SetRunState(bool bRun);
+
+namespace iv {
+struct ndttracepoint
+{
+    double x;
+    double y;
+    double z;
+    double pitch;
+    double roll;
+    double yaw;
+};
+}
+
+namespace iv {
+struct gpspos
+{
+    double lat;
+    double lon;
+    double height;
+    double heading;
+    double pitch;
+    double roll;
+    double ve;
+    double vn;
+};
+}
+
+iv::gpspos PoseToGPS(iv::gpspos xori,pose xpose);
+pose CalcPose(iv::gpspos xori,iv::gpspos xcur);
+
+namespace iv {
+struct ndtmaptrace
+{
+    ndtmaptrace() {}
+    std::vector<ndttracepoint> mvector_trace;
+    std::string mstrpcdpath;
+    iv::gpspos mndtorigin;
+};
+
+}
+
+void setuseimu(bool bUse);
+void imu_update(qint64 current_time_imu,double imu_roll,double imu_pitch,double imu_yaw,
+                       double acceleration_x,double acceleration_y,
+                       double acceleration_z);
+
+#endif // NDT_MATCHING_H

src/detection/detection_ndt_pclomp/detection_ndt_pclomp.pro

@@ -88,6 +88,7 @@ unix:LIBS +=  -lpcl_common\
     error( "Couldn't find the common.pri file!" )
 }
 
+#DEFINES+= TEST_CALCTIME
 
 INCLUDEPATH += $$PWD/../../../include/
 LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault -lndt_gpu  -livexit -livbacktrace

src/detection/detection_ndt_pclomp/ndt_matching.cpp

@@ -1204,6 +1204,10 @@ void imu_update(qint64 current_time_imu,double imu_roll,double imu_pitch,double
 }
 
 
+#ifdef TEST_CALCTIME
+int ncalc = 0;
+int gncalctotal = 0;
+#endif
 
 void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
 {
@@ -1447,6 +1451,17 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
       else
           std::cout<<RED<<"after ndt time is "<<xTime.elapsed()<<RESET<<std::endl;
 
+#ifdef TEST_CALCTIME
+
+    gncalctotal = gncalctotal + xTime.elapsed();
+    ncalc++;
+    if(ncalc>0)
+    {
+        std::cout<<" average calc time: "<<gncalctotal/ncalc<<std::endl;
+
+    }
+#endif
+
       has_converged = ndt_omp->hasConverged();
 
       t = ndt_omp->getFinalTransformation();