Merge branch 'master' of http://116.63.46.168:3000/ADPilot/modularization

include/common.pri

@@ -21,7 +21,7 @@ LIBS += -L$$PWD/../bin/ -lxmlparam -lmodulecomm  -livlog -livfault -livexit -liv
 
 unix:LIBS += -lboost_system  -lbacktrace -ldl
 
-QMAKE_CXXFLAGS +=  -g
+#QMAKE_CXXFLAGS +=  -g
 
 CONFIG += c++11
 

src/chassis_test/shenlan_test/mainwindow.cpp

@@ -45,6 +45,8 @@ MainWindow::MainWindow(QWidget *parent) :
     decition_debug.set_wheelangle(5);
     decition_debug.set_torque(1);
     decition_debug.set_brake(2);
+    decition_debug.set_leftlamp(0);
+    decition_debug.set_rightlamp(0);
 
 }
 
@@ -105,6 +107,21 @@ void MainWindow::ShareChassisDebug()
     char * str = new char[nsize];
     std::shared_ptr<char> pstr;
     pstr.reset(str);
+    if(ui->left_turn->isChecked())
+    {
+        decition_debug.set_leftlamp(1);
+        decition_debug.set_rightlamp(0);
+    }
+    else if(ui->right_turn->isChecked())
+    {
+        decition_debug.set_leftlamp(0);
+        decition_debug.set_rightlamp(1);
+    }
+    else
+    {
+        decition_debug.set_leftlamp(0);
+        decition_debug.set_rightlamp(0);
+    }
     if(decition_debug.SerializeToArray(str,nsize))
     {
         if(ui->start_send->isChecked())
@@ -155,3 +172,26 @@ void MainWindow::on_brake_valueChanged(double arg1)
       decition_debug.set_brake(arg1);
     }
 }
+
+void MainWindow::on_left_turn_clicked()
+{
+
+}
+void MainWindow::on_left_turn_clicked(bool checked)
+{
+//    if(checked)
+//    {
+//        decition_debug.set_leftlamp(1);
+//        decition_debug.set_rightlamp(0);
+//    }
+//    else
+//        decition_debug.set_leftlamp(0);
+}
+
+void MainWindow::on_right_turn_clicked(bool checked)
+{
+//    if(checked)
+//        decition_debug.set_rightlamp(1);
+//    else
+//        decition_debug.set_rightlamp(0);
+}

src/chassis_test/shenlan_test/mainwindow.h

@@ -25,6 +25,12 @@ private slots:
 
      void on_brake_valueChanged(double arg1);
 
+     void on_left_turn_clicked();
+
+     void on_left_turn_clicked(bool checked);
+
+     void on_right_turn_clicked(bool checked);
+
 public:
     explicit MainWindow(QWidget *parent = 0);
     ~MainWindow();

src/chassis_test/shenlan_test/mainwindow.ui

@@ -1599,8 +1599,8 @@
  </resources>
  <connections/>
  <buttongroups>
-  <buttongroup name="engineer_buttonGroup"/>
-  <buttongroup name="gear_buttonGroup"/>
   <buttongroup name="EPB_buttonGroup"/>
+  <buttongroup name="gear_buttonGroup"/>
+  <buttongroup name="engineer_buttonGroup"/>
  </buttongroups>
 </ui>

src/common/common/xodr/xodrfunc/xodrfunc.cpp

@@ -224,6 +224,8 @@ double xodrfunc::GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
 
     double hdgltoa = CalcHdg(QPointF(x,y),QPointF(x_center,y_center));
 
+//    double hdgltoa = CalcHdg(QPointF(x_center,y_center),QPointF(x,y));
+
 
     QPointF arcpoint;
     arcpoint.setX(x_center);arcpoint.setY(y_center);

src/common/ivbacktrace/ivbacktrace.cpp

@@ -274,12 +274,12 @@ void out_stack(char *sig)
 
 void RegisterIVBackTrace()
 {
-    signal(SIGHUP, signal_exit);
-    signal(SIGINT, signal_exit);
-    signal(SIGQUIT, signal_exit);
+//    signal(SIGHUP, signal_exit);
+//    signal(SIGINT, signal_exit);
+//    signal(SIGQUIT, signal_exit);
     signal(SIGABRT, signal_exit);
-    signal(SIGKILL, signal_exit);
-    signal(SIGTERM, signal_exit);
+//    signal(SIGKILL, signal_exit);
+//    signal(SIGTERM, signal_exit);
     signal(SIGSEGV, signal_exit);
 }
 

src/common/modulecomm/modulecomm.pro

@@ -27,7 +27,7 @@ QT += dbus
 DEFINES += USEDBUS
 }
 
-
+QMAKE_CXXFLAGS +=  -g
 
 CONFIG += c++11
 

src/common/modulecomm/shm/procsm.cpp

@@ -124,13 +124,17 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
     mpASMPtr = new QSharedMemory(strsmname);
     mpthreadattmon = new std::thread(&procsm::threadAttachMon,this);
 
+//
+
     char strasmname[300];
+    snprintf(strasmname,255,"%s_%lld",strsmname,std::chrono::system_clock::now().time_since_epoch().count());
 
 
     mnMode = nMode;
     if(nMode == ModeWrite)
     {
         int nrtn = CreateASMPTR(strasmname,nBufSize,nMaxPacCount);
+//        int nrtn = CreateASMPTR(mstrsmname,nBufSize,nMaxPacCount);
         if(nrtn<0)
         {
             std::cout<<"CreateASMPTR Fail."<<std::endl;
@@ -143,11 +147,15 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
         return;
     }
 
+
+
 //    mpASM = new QSharedMemory(strsmname);
     mpASM = new QSharedMemory(strasmname);
 
     if(nMode == ModeWrite)
     {
+
+
         strncpy(mmodulemsg_type.mstrmsgidname,strsmname,255);
         mmodulemsg_type.mnBufSize = nBufSize;
         mmodulemsg_type.mnMsgBufCount = nMaxPacCount;
@@ -164,6 +172,7 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
 #endif
 #endif
 
+        std::cout<<" create asm impl name: "<<strasmname<<std::endl;
         int nrtn = CreateAndAttachASM(strasmname,nBufSize,nMaxPacCount,strsmname);
         if(nrtn <0 )
         {
@@ -172,6 +181,7 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
             ivstdcolorout(strerr,iv::STDCOLOR_BOLDRED);
         }
 
+
     }
 
     mbInitComplete = true;
@@ -225,7 +235,7 @@ int procsm::CreateASMPTR(char * strasmname,const unsigned int nBufSize,const uns
         }
         else
         {
-            std::cout<<"Attach Fail."<<std::endl;
+            std::cout<<"Attach "<<mstrsmname<<" Attach Fail."<<std::endl;
             return -1;
         }
     }
@@ -535,6 +545,11 @@ bool procsm::AttachMem()
                 }
 
             }
+            else
+            {
+                mpinfo = NULL;
+                mphead = NULL;
+            }
 
 
             return false;
@@ -758,6 +773,12 @@ unsigned int procsm::getcurrentnext()
 {
 
     checkasm();
+
+    if(mpinfo == NULL)
+    {
+        std::cout<<"procsm::getcurrentnext attach asm fail.  "<<std::endl;
+        return 0;
+    }
     unsigned int nNext;
     mpASM->lock();
     nNext = mpinfo->mNext;

src/common/ndt_cpu/ndt_cpu.pro

@@ -25,6 +25,7 @@ unix:INCLUDEPATH += /usr/include/pcl-1.8
 unix:INCLUDEPATH += /usr/include/pcl-1.7
 
 unix:INCLUDEPATH += /usr/include/pcl-1.12
+unix:INCLUDEPATH += /usr/include/pcl-1.10
 
 
 #PKGCONFIG +=pcl

src/common/ndt_gpu/ndt_gpu.pro

@@ -20,9 +20,11 @@ QMAKE_CXXFLAGS += -std=gnu++17
 
 SOURCES +=  \
 
+#INCLUDEPATH += /usr/local/cuda-10.2/targets/aarch64-linux/include/crt
 #INCLUDEPATH += /home/nvidia/eigentem
 unix:INCLUDEPATH += /usr/include/eigen3
 unix:INCLUDEPATH += /usr/include/pcl-1.8
+unix:INCLUDEPATH += /usr/include/pcl-1.10
 
 unix:LIBS += -lboost_thread -lboost_system
 
@@ -53,15 +55,15 @@ LIBS += -L"/usr/local/lib" \
         -lcudart \
         -lcufft
 
-CUDA_SDK = "/usr/local/cuda-10.2/"   # cudaSDK路径
+CUDA_SDK = "/usr/local/cuda/"   # cudaSDK路径
 
-CUDA_DIR = "/usr/local/cuda-10.2/"            # CUDA tookit路径
+CUDA_DIR = "/usr/local/cuda/"            # CUDA tookit路径
 
 SYSTEM_NAME = linux         # 自己系统环境 'Win32', 'x64', or 'Win64'
 
 SYSTEM_TYPE = 64           #操作系统位数 '32' or '64',
 
-CUDA_ARCH = sm_72          # cuda架构, for example 'compute_10', 'compute_11', 'sm_10'
+CUDA_ARCH = sm_72  #sm_72  # xavier 72  orin 87          # cuda架构, for example 'compute_10', 'compute_11', 'sm_10'
 
 NVCC_OPTIONS = --use_fast_math --compiler-options "-fPIC"
 

src/controller/controller_changan_shenlan_v2/controller_changan_shenlan_v2.pro

@@ -40,7 +40,7 @@ include($$PWD/control/control.pri)
 INCLUDEPATH += $$PWD/../controllercommon
 
 
-#DEFINES += TORQUEBRAKETEST
+DEFINES += TORQUEBRAKETEST
 
 #unix:!macx: LIBS += -L/home/yuchuli/qt/lib -lncomgnss -ladcmemshare
 

src/controller/controller_changan_shenlan_v2/main.cpp

@@ -48,6 +48,11 @@ static int gnIndex = 0;
 static bool gbHaveVehSpd = false;
 static double gfVehSpd = 0.0;
 
+static bool gbHaveWheelAngle = false;
+static double gfWheelAngle = 0.0;
+
+static double gfWheelSpeedLim = 300; //300 degrees per second
+
 static boost::shared_ptr<iv::control::Controller> gcontroller;	//实际车辆控制器
 
 static QMutex gMutex;
@@ -114,29 +119,53 @@ typedef struct
 
 typedef struct
 {
+    uint8_t CdcDoor;
+    uint8_t res1;
+    uint8_t res2;
     uint8_t ADS_UDLCTurnLightReq;
-} ECU_36E_t;
+} ECU_25E_t;  //zhuanxiangdeng IDgenghuan
 
 unsigned char byte_1C4[64];//byte_144[8];
 unsigned char byte_24E[64];
-unsigned char byte_36E[64];
+unsigned char byte_25E[32];
 
 ECU_1C4_t _m1C4 = {0,0,0,0};
 ECU_24E_t _m24E = {0,0,0,0,0,0,0,0,0,0,0};
-ECU_36E_t _m36E = {0};
+ECU_25E_t _m25E = {0,0,0,0};
 
 void ExecSend();
 
 void executeDecition(const iv::brain::decition &decition)
 {
 
+
+
+
     static int xieya = 50;
 //     std::cout<<"acc is "<<decition.torque()<<" ang is "<<decition.wheelangle()<<std::endl;
 //     std::cout<<"angle_mode is "<<decition.angle_mode()<<" angle_active is "<<decition.angle_active()<<std::endl;
 //     std::cout<<"brake_type is "<<decition.brake_type()<<" acc_active is "<<decition.acc_active()<<std::endl;
 //     std::cout<<"brake is "<<decition.brake()<<" brake_active is "<<decition.brake_active()<<std::endl;
 //     std::cout<<"auto_mode is "<<decition.auto_mode()<<" rightlamp is "<<decition.rightlamp()<<std::endl;
-    _m1C4.ACC_LatAngReq = ECU_1C4_ACC_LatAngReq_toS(decition.wheelangle());
+
+    double fWheelAngleReq = decition.wheelangle();
+    double fsendinter = 0.02;
+//    if(fabs(fWheelAngleReq - gfWheelAngle)/fsendinter > gfWheelSpeedLim)
+//    {
+//        if(fWheelAngleReq > gfWheelAngle)
+//        {
+//            fWheelAngleReq = gfWheelAngle + fsendinter * gfWheelSpeedLim;
+//        }
+//        else
+//        {
+//            fWheelAngleReq = gfWheelAngle - fsendinter * gfWheelSpeedLim;
+//        }
+
+//    }
+
+//    std::cout<<" wheel req: "<<decition.wheelangle()<<" real send : "<<fWheelAngleReq<<" real whhel:"<<gfWheelAngle<< std::endl;
+
+    _m1C4.ACC_LatAngReq = ECU_1C4_ACC_LatAngReq_toS(fWheelAngleReq);
     //_m1C4.ADS_Reqmode = decition.angle_mode(); //20221102,新车没有此信号
     _m1C4.ACC_MotorTorqueMaxLimitRequest = ECU_1C4_ACC_MotorTorqueMaxLimitRequest_toS(10);
     _m1C4.ACC_LatAngReqActive = decition.angle_active();
@@ -313,14 +342,14 @@ void executeDecition(const iv::brain::decition &decition)
     //byte_24E[13] = ((_m24E.ACC_AccTrqReq & (0x1FU)<<3))| ((_m24E.ACC_AccTrqReqActive & (0x01U)) << 2);
     byte_24E[13] = (((_m24E.ACC_AccTrqReq & (0x1FU))<<3))| ((_m24E.ACC_AccTrqReqActive & (0x01U)) << 2);
 
-//    if(decition.leftlamp() == true && decition.rightlamp() == false)
-//        _m36E.ADS_UDLCTurnLightReq = 3;
-//    else if(decition.leftlamp() == false && decition.rightlamp() == true)
-//        _m36E.ADS_UDLCTurnLightReq = 4;
-//    else
-//        _m36E.ADS_UDLCTurnLightReq = 0;
+    if(decition.leftlamp() == true && decition.rightlamp() == false)
+        _m25E.ADS_UDLCTurnLightReq = 3;
+    else if(decition.leftlamp() == false && decition.rightlamp() == true)
+        _m25E.ADS_UDLCTurnLightReq = 4;
+    else
+        _m25E.ADS_UDLCTurnLightReq = 0;
 
-//    byte_36E[0] = ((_m36E.ADS_UDLCTurnLightReq & (0x07U)) << 5);
+    byte_25E[3] = ((_m25E.ADS_UDLCTurnLightReq & (0x07U)));
 
 }
 
@@ -418,6 +447,12 @@ void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned
         gbHaveVehSpd = true;
   //      std::cout<<" gf Veh speed : "<<gfVehSpd<<std::endl;
     }
+
+    if(xchassis.has_angle_feedback())
+    {
+        gfWheelAngle = xchassis.angle_feedback();
+        gbHaveWheelAngle = true;
+    }
 }
 
 
@@ -435,6 +470,8 @@ void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned
         return;
     }
 
+
+
     //    if(xdecition.gear() != 4)
     //    {
     //        qDebug("not D");
@@ -492,19 +529,20 @@ void ExecSend()
     xmsg.set_channel(0);
     xmsg.set_index(gnIndex);
 
-//    xraw.set_id(0x36E);
-//    xraw.set_data(byte_36E,8);
-//    xraw.set_bext(false);
-//    xraw.set_bremote(false);
-//    xraw.set_len(8);
+    xraw.set_id(0x25E);
+    xraw.set_data(byte_25E,32);
+    xraw.set_bext(false);
+    xraw.set_bremote(false);
+    xraw.set_len(32);
 //    if(nCount == 10)
-//    {
-//        iv::can::canraw * pxraw36E = xmsg.add_rawmsg();
-//        pxraw36E->CopyFrom(xraw);
+    if(nCount%2 == 1) //25Ede zhouqi shi 20ms
+    {
+        iv::can::canraw * pxraw25E = xmsg.add_rawmsg();
+        pxraw25E->CopyFrom(xraw);
 //        nCount = 0;
-//    }
-//    xmsg.set_channel(0);
-//    xmsg.set_index(gnIndex);
+    }
+    xmsg.set_channel(0);
+    xmsg.set_index(gnIndex);
 
     gnIndex++;
     xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
@@ -565,6 +603,34 @@ void sendthread()
             gMutex.unlock();
             gnDecitionNum--;
         }
+
+#ifdef TORQUEBRAKETEST
+        if(gnothavetb < 10)
+        {
+            iv::controller::torquebrake xtb;
+            gMutextb.lock();
+            xtb.CopyFrom(gtb);
+            gMutextb.unlock();
+            if(xtb.enable())
+            {
+
+                xdecition.set_torque(xtb.torque());
+                xdecition.set_brake(xtb.brake());
+
+                std::cout<<" use xtb. torque: "<<xtb.torque()<<" brake: "<<xtb.brake()<<std::endl;
+
+//                gcontroller->control_torque(xtb.torque());
+//                gcontroller->control_brake(xtb.brake());
+                //            qDebug("use tb value torque is %f brake is %f",xtb.torque(),xtb.brake());
+            }
+            else
+            {
+                //            qDebug("torquebrake not enable.");
+            }
+            gnothavetb++;
+        }
+
+#endif
         executeDecition(xdecition);
         if(gbChassisEPS == false) ExecSend();
         std::this_thread::sleep_for(std::chrono::milliseconds(10));

src/decition/common/common/car_status.h

@@ -269,6 +269,10 @@ namespace iv {
         double ang_debug=0.0;
         double torque_or_acc=0.0;
         double brake=0.0;
+
+
+        double mfbasepitch = 0.0;
+        bool mbUseRampAssit = false;
     };
     typedef boost::serialization::singleton<iv::CarStatus> CarStatusSingleton;//非线程安全，注意多线程加锁，单例模式
 }

src/decition/common/perception_sf/impl_gps.cpp

@@ -324,6 +324,11 @@ void iv::perception::GPSSensor::UpdateGPSIMU(iv::gps::gpsimu xgpsimu)
 
     data->speed = sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)) * 3.6;
 
+    if(xgpsimu.has_speed())
+    {
+        data->speed =  xgpsimu.speed() * 3.6;
+    }
+
     GaussProjCal(data->gps_lng, data->gps_lat, &data->gps_x, &data->gps_y);
 
     ServiceCarStatus.mRTKStatus = data->rtk_status;

src/decition/common/perception_sf/sensor_gps.cpp

@@ -1,22 +1,52 @@
 #include <perception_sf/sensor_gps.h>
 #include "ivlog.h"
 
+#include "chassis.pb.h"
+
 extern std::string gstrmemgps;
 extern iv::Ivlog * givlog;
 
+
+
 namespace iv {
 namespace perception {
     GPSSensor * ggpsimu;
+    static int64_t mlastfusiongpsimutime;
+
+
+
+
+
 
     void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
     {
 //        ggpsimu->UpdateGPSIMU(strdata,nSize);
 
+        if(strcmp(strmemname,"fusion_gpsimu") == 0)
+        {
+            mlastfusiongpsimutime = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+        }
+        else
+        {
+            int64_t nmsnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+            if(abs(nmsnow - mlastfusiongpsimutime) < 3000)
+            {
+                std::cout<<" fusion_gpsimu is ok. use fusiongpsimu."<<std::endl;
+                return;
+            }
+        }
+
+
+
+
+
         iv::gps::gpsimu xgpsimu;
         if(!xgpsimu.ParseFromArray(strdata,nSize))
         {
             std::cout<<"ListenRaw Parse error."<<std::endl;
         }
+
+
 //        qDebug(" gps time is %ld",QDateTime::currentMSecsSinceEpoch());
         ggpsimu->UpdateGPSIMU(xgpsimu);
 
@@ -36,11 +66,13 @@ namespace perception {
 iv::perception::GPSSensor::GPSSensor() {
 	signal_gps_data = createSignal<sig_cb_gps_sensor_data>();
     ggpsimu = this;
+    mlastfusiongpsimutime = 0;
 }
 iv::perception::GPSSensor::~GPSSensor() {
 
     thread_sensor_run_->interrupt();
     iv::modulecomm::Unregister(mpagpsimu);
+    iv::modulecomm::Unregister(mpafusiongpsimu);
     std::cout<<"~GPSSensor()"<<std::endl;
 }
 
@@ -49,6 +81,7 @@ void iv::perception::GPSSensor::start()
     thread_sensor_run_ = new boost::thread(boost::bind(&iv::perception::GPSSensor::processSensor, this));
 
     mpagpsimu = iv::modulecomm::RegisterRecv(gstrmemgps.data(),iv::perception::Listengpsimu);
+    mpafusiongpsimu = iv::modulecomm::RegisterRecv("fusion_gpsimu",iv::perception::Listengpsimu);
     //OutputDebugString(TEXT("\nGPS thread Run\n"));
 }
 

src/decition/common/perception_sf/sensor_gps.h

@@ -210,6 +210,8 @@ namespace iv {
 
         private:
             void * mpagpsimu;
+            void * mpafusiongpsimu;
+
 
         public:
             void UpdateGPSIMU(iv::gps::gpsimu xgpsimu);

src/decition/decition_brain_sf_changan_shenlan/decition/adc_adapter/changan_shenlan_adapter.cpp

@@ -22,6 +22,7 @@ iv::decition::ChanganShenlanAdapter::~ChanganShenlanAdapter(){
 iv::decition::Decition iv::decition::ChanganShenlanAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
                                                                               float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
 
+    bool bUseDynamics = false;
 
     float controlSpeed=0;
     float controlBrake=0;
@@ -53,11 +54,34 @@ iv::decition::Decition iv::decition::ChanganShenlanAdapter::getAdapterDeciton(GP
     }
     else
     {
+
+
         controlBrake = 0;
-        if(lastTorque==0){
-            controlSpeed=100;
-        }else{
-            controlSpeed = lastTorque+(accAim-accNow)*500*0.1;
+        if(bUseDynamics == false)
+        {
+            if(lastTorque==0){
+                controlSpeed=100;
+            }else{
+                controlSpeed = lastTorque+(accAim-accNow)*500*0.1;
+            }
+        }
+        else
+        {
+            double fVehWeight = 1800;
+            double fg = 9.8;
+            double fRollForce = 50;
+            const double fRatio = 2.5;
+            double fNeed = fRollForce + fVehWeight*accAim;
+            if(ServiceCarStatus.mbUseRampAssit)
+            {
+                if(now_gps_ins.ins_pitch_angle>(ServiceCarStatus.mfbasepitch + 1.0))
+                {
+                    double fpitch = now_gps_ins.ins_heading_angle - ServiceCarStatus.mfbasepitch;
+                    double fpitchforce = fVehWeight* fg * sin(fpitch*M_PI/180.0);
+                    fNeed = fNeed + fpitchforce;
+                }
+            }
+            controlSpeed = fNeed/fRatio;
         }
     }
 
@@ -80,13 +104,13 @@ iv::decition::Decition iv::decition::ChanganShenlanAdapter::getAdapterDeciton(GP
     controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
 
     if(DecideGps00::minDecelerate<0){
-        controlBrake = max((0-DecideGps00::minDecelerate)*30,controlBrake);
+        controlBrake = max((0-DecideGps00::minDecelerate)*20,controlBrake);
         controlSpeed=0;
     }
     controlBrake=min(controlBrake,100.0f);
     controlBrake=max(controlBrake,0.0f);
 
-        (*decition)->brake = 0 - controlBrake/25;
+        (*decition)->brake = 0 - controlBrake/20;
         (*decition)->torque= controlSpeed;
         lastTorque=(*decition)->torque;
 
@@ -183,6 +207,10 @@ float iv::decition::ChanganShenlanAdapter::limitSpeed(float realSpeed, float con
         controlSpeed=0;
     }
 
+    controlSpeed=min((float)3000,controlSpeed);
+    controlSpeed=max((float)0.0,controlSpeed);
+    return controlSpeed;
+
     if(realSpeed<10){
         if(realSpeed<5)
         {

src/decition/decition_brain_sf_changan_shenlan/decition/adc_adapter/hunter_adapter.cpp

@@ -0,0 +1,304 @@
+#include <decition/adc_adapter/hunter_adapter.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+
+#include "ivlog.h"
+extern iv::Ivlog * givlog;
+
+using namespace std;
+
+
+iv::decition::HunterAdapter::HunterAdapter(){
+    adapter_id=10;
+    adapter_name="hunter";
+}
+iv::decition::HunterAdapter::~HunterAdapter(){
+
+}
+
+//hunter 只用控制转角和速度，速度大于0前进，速度小于0后退，驻车是在完全停车的时候再用
+
+iv::decition::Decition iv::decition::HunterAdapter::getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistance ,
+                                                                        float  obsSpeed,float accAim,float accNow  ,bool changingDangWei,Decition *decition){
+    accAim=min(0.7f,accAim);
+
+    float controlSpeed=0;
+    float controlBrake=0;
+    float u = 0- accAim;
+    float realSpeed = now_gps_ins.speed;
+    float ttc = 0-(obsDistance/obsSpeed);
+    bool turn_around=false;
+    if(now_gps_ins.roadMode==14||now_gps_ins.roadMode==15||now_gps_ins.roadMode==16||now_gps_ins.roadMode==17){
+        turn_around=true;
+    }
+
+    if(ttc<0){
+        ttc=15;
+    }
+//    if (accAim < 0)
+//    {
+//        controlSpeed=0;
+//        controlBrake=u; //102
+////        if(obsDistance>0 && obsDistance<10)
+//        if(obsDistance>0 && obsDistance<6)
+//        {
+//            controlBrake= u*1.0;     //1.5    zj-1.2
+//            controlSpeed=0;
+//        }
+////        if(abs(dSpeed-realSpeed)<1 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && !turn_around)
+//        if(ttc<5)
+//        {
+//            controlBrake=0;
+//            controlSpeed=0;
+////            controlSpeed=max(0.0,realSpeed-1.0);
+//        }
+////        else if(abs(dSpeed-realSpeed)<3 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around
+////                 && dSpeed>0 && lastBrake==0)
+//        else if(ttc<10)
+//        {
+//            controlBrake=0;
+//            controlSpeed=max(0.0,realSpeed-2.0);
+//        }
+////        else if(abs(realSpeed)<15 &&(obsDistance>40 || obsDistance<0)&&ttc>8 && dSpeed>0 && turn_around)
+//        else
+//        {
+//            controlBrake=0;
+//            controlSpeed=max(0.0,realSpeed-1.0);
+//        }
+
+////       else if(abs(dSpeed-realSpeed)>=5 &&abs(dSpeed-realSpeed)<10&&(obsDistance>40 || obsDistance<0)&&ttc>8
+////                  && !turn_around )
+////        {
+////            controlBrake=min(controlBrake,0.3f);
+////            controlSpeed=0;
+////        }
+////        else if(abs(dSpeed-realSpeed)>=10 &&abs(dSpeed-realSpeed)<15&&(obsDistance>40 || obsDistance<0)&&ttc>8
+////                && dSpeed>0 && !turn_around )
+////        {
+////            controlBrake=min(controlBrake,0.5f);
+////            controlSpeed=0;
+////        }
+
+
+//        //0110
+//        if(changingDangWei){
+//            controlBrake=max(0.5f,controlBrake);
+//        }
+
+//        //斜坡刹车加大 lsn 0217
+//        if (abs(now_gps_ins.ins_pitch_angle)>2.5 &&(controlBrake>0||dSpeed==0)){
+//            controlBrake=max(2.0f,controlBrake);
+//        }
+//        //斜坡刹车加大 end
+//    }
+//    else
+//    {
+//        controlBrake = 0;
+
+//        if(abs(dSpeed-realSpeed)<=2.0)
+//        {
+//            controlSpeed= dSpeed;
+//        }
+//        else if(abs(dSpeed-realSpeed)>2.0)
+//        {
+//             controlSpeed = min(realSpeed+1,dSpeed);
+//        }
+
+         controlSpeed= dSpeed;
+
+
+//        controlSpeed= dSpeed;
+
+        // 斜坡加大油门   0217 lsn//斜坡的hunter 不考虑，20210913，cxw
+
+//    }
+
+    controlSpeed= dSpeed;
+//    controlSpeed=max(controlSpeed,5.0f); //对车子进行限速，车子最大速度是5km/h
+    if(controlSpeed>5.0)
+    {
+        controlSpeed=5.0;
+    }
+
+    //0227 10m nei xianzhi shache
+    //障碍物距离在3~10米之间，让速度慢慢降到1，当障碍物距离小于3了，直接停车
+    if(obsDistance<10 &&obsDistance>3){
+        controlSpeed=max(1.0,realSpeed-0.5);
+        controlBrake=max(controlBrake,0.6f);//0.8   zj-0.6
+    }
+    else if(obsDistance<=3 &&obsDistance>0)
+    {
+        controlSpeed=0;
+        controlBrake=max(controlBrake,0.6f);//0.8   zj-0.6
+    }
+
+    if(DecideGps00::minDecelerate<0)  //在到达地图终点的时候，停车
+    {
+        controlBrake = max((0-DecideGps00::minDecelerate),controlBrake);
+        controlSpeed=0;
+    }
+
+//    if(DecideGps00::minDecelerate==-0.4){
+//        controlBrake =0.4;
+//    }
+
+    controlBrake=limitBrake(realSpeed,controlBrake,dSpeed,obsDistance,ttc);
+    controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
+
+    double reverse_speed=-(realSpeed+8)/3.6;//avoid veh in the reverse road
+//    if((obsDistance>8.0)&&(obsSpeed<reverse_speed)){
+//        controlBrake =0;
+//        controlSpeed =0;
+//    }
+
+
+    (*decition)->brake = controlBrake*9;//9     zj-6//对于hunter车辆，brake 没有用
+    if(controlSpeed==0.0)
+    {
+        givlog->debug("brain_decition","controlSpeed: %f", controlSpeed);
+    }
+    (*decition)->torque =controlSpeed;//hunter目标速度大于0前进，小于0后退，hunter只有速度，角度和驻车控制
+
+    lastBrake= (*decition)->brake;
+    lastTorque=(*decition)->torque;
+
+
+    givlog->debug("brain_decition","brake: %f,obsDistance: %f,obsSpeed: %f,reverse_speed: %f",
+                            (*decition)->brake,obsDistance,obsSpeed,reverse_speed,ttc);
+
+    givlog->debug("brain_decition","dSpeed: %f,realspeed: %f,brake: %f,torque: %f",
+                            dSpeed,realSpeed,(*decition)->brake,(*decition)->torque);
+
+
+
+
+    (*decition)->grade=1;
+    (*decition)->mode=1;
+    (*decition)->speak=0;
+    (*decition)->handBrake=1;  // 0: laqi 1: shifang
+    (*decition)->bright=false;
+    (*decition)->engine=0;
+
+    (*decition)->dangweiEnable=true;
+    (*decition)->angleEnable=true;
+    (*decition)->speedEnable=true;
+    (*decition)->brakeEnable=true;
+    (*decition)->air_enable = false;
+    (*decition)->driveMode=1;
+    (*decition)->brakeType=0;
+    (*decition)->angSpeed=60;
+    (*decition)->topLight=0;
+
+
+    if(ServiceCarStatus.bocheMode ||ServiceCarStatus.daocheMode){
+        (*decition)->dangWei=2;
+        (*decition)->backLight=1;
+    }
+    //1220
+    else{
+        (*decition)->dangWei=4;
+        (*decition)->backLight=0;
+    }
+
+
+
+    if((*decition)->brake>0){
+        (*decition)->brakeLight=1;
+    }else{
+        (*decition)->brakeLight=0;
+    }
+
+
+    if((*decition)->leftlamp){
+        (*decition)->directLight=1;
+    }else if((*decition)->rightlamp){
+         (*decition)->directLight=2;
+    }else{
+        (*decition)->directLight=0;
+    }
+
+    (*decition)->handBrake=false;
+    (*decition)->wheel_angle+=ServiceCarStatus.mfEPSOff;
+
+    (*decition)->wheel_angle=max((float)-870.0,(*decition)->wheel_angle);
+    (*decition)->wheel_angle=min((float)870.0,(*decition)->wheel_angle);
+
+
+
+
+        //dangweiTrasfer
+//             if((decition_gps->dangWei!=ServiceCarStatus.dangwei_st && ServiceCarStatus.dangwei_st!=0)){
+//                 decition_gps->dangWei=2;
+//                 decition_gps->torque=0;
+//             }
+     lastDangWei= (*decition)->dangWei;
+
+    (*decition)->topLight=0; //1116
+    (*decition)->nearLight=0;
+    (*decition)->farLight=0;
+    (*decition)->door=3;
+
+std::cout<<"==========================================="<<std::endl;
+     std::cout<<"dSpeed:"<<dSpeed<<"   realSpeed:"<<realSpeed<<"   acc:"<<accAim<<"  torque_st:"<<ServiceCarStatus.torque_st
+             <<"  decideTorque:"<<(*decition)->torque <<"  decideBrake:"<<(*decition)->brake
+    <<std::endl;
+std::cout<<"========================================="<<std::endl;
+
+
+   (*decition)->accelerator=  (*decition)->torque ;
+
+//将ttc时间和障碍物存储到log中方便后期数据分析
+
+
+    return *decition;
+}
+
+
+
+float iv::decition::HunterAdapter::limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc){
+
+    //刹车限制
+
+    //刹车限制
+
+    float vs =realSpeed*3.6;
+    int BrakeIntMax = 10;
+    if (vs < 10.0 / 3.6) BrakeIntMax = 4;
+    else if (vs < 20.0 / 3.6) BrakeIntMax = 6;
+
+    //`
+    if(ttc>10){
+        BrakeIntMax = 2;
+    }else if(ttc>6){
+        BrakeIntMax = 3;
+    }else if(ttc>3){
+        BrakeIntMax = 4;
+    }else {
+        BrakeIntMax = 5;
+    }
+    if(obsDistance>0 && obsDistance<10){
+        BrakeIntMax=max(BrakeIntMax,3);
+    }
+
+    if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
+
+
+
+
+    controlBrake=min(5.0f,controlBrake);
+    controlBrake=max(0.0f,controlBrake);
+    return controlBrake;
+
+}
+
+float iv::decition::HunterAdapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
+
+    controlSpeed=min((float)100.0,controlSpeed);
+    controlSpeed=max((float)0.0,controlSpeed);
+    return controlSpeed;
+}
+
+

src/decition/decition_brain_sf_changan_shenlan/decition/adc_adapter/hunter_adapter.h

@@ -0,0 +1,40 @@
+#ifndef HUNTER_ADAPTER_H
+#define HUNTER_ADAPTER_H
+#include <common/gps_type.h>
+#include <decition/decition_type.h>
+#include <common/obstacle_type.h>
+#include <vector>
+#include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <decition/adc_adapter/base_adapter.h>
+#include <decition/adc_tools/transfer.h>
+#include <decition/decide_gps_00.h>
+
+
+namespace iv {
+     namespace decition {
+         class HunterAdapter: public BaseAdapter {
+                    public:
+
+                        float lastTorque;
+                        float lastBrake;
+                        int lastDangWei=0;
+
+
+                        HunterAdapter();
+                        ~HunterAdapter();
+
+                        iv::decition::Decition getAdapterDeciton(GPS_INS now_gps_ins, std::vector<Point2D>  trace , float dSpeed, float obsDistacne ,
+                                                                                 float  obsSpeed,float acc,float accNow  ,bool changingDangWei,Decition *decition);
+
+                        float limitBrake(float realSpeed, float controlBrake,float dSpeed,float obsDistance , float ttc);
+                        float limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed );
+
+                    private:
+                        int seizingCount=0;
+
+            };
+
+    }
+}
+
+#endif // HUNTER_ADAPTER_H

src/decition/decition_brain_sf_changan_shenlan/decition/adc_controller/pid_controller.cpp

@@ -71,6 +71,31 @@ double iv::decition::PIDController::CalcKappa(std::vector<Point2D> trace,int pre
 
 }
 
+double iv::decition::PIDController::ExtendPreviewDistance(std::vector<Point2D> & trace,double & PreviewDistance)
+{
+    double fAllowableError = 0.3;
+    int i = 0;
+    int nsize = static_cast<int>(trace.size());
+    double fPreviewAllow = 0;
+    for(i=0;i<(nsize-1);i++)
+    {
+        if(fabs(trace[i].x)>fAllowableError)
+        {
+            break;
+        }
+    }
+    if((i>=0)&&(i<nsize))
+    {
+        fPreviewAllow = fabs(trace[i].y);
+    }
+    if(fPreviewAllow > PreviewDistance)
+    {
+        PreviewDistance = fPreviewAllow;
+    }
+    return fPreviewAllow;
+
+}
+
 float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Point2D> trace, int roadMode){
     double ang = 0;
     double EPos = 0, EAng = 0;
@@ -94,6 +119,17 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
         realSpeed > 40 ? PreviewDistance = max(6.0, realSpeed *0.6) : PreviewDistance = max(4.0, realSpeed *0.3);
     }
 
+    if(ServiceCarStatus.msysparam.mvehtype=="hunter")
+    {
+        PreviewDistance = 3.0;
+    }
+
+    bool bExtendPreview = true;
+    if(bExtendPreview)
+    {
+        ExtendPreviewDistance(trace,PreviewDistance);
+    }
+
 
     //    if(ServiceCarStatus.msysparam.mvehtype=="bus"){
     //        KEang = 14, KEPos = 7, DEang = 0, DEPos = 0,IEpos=0,IEang=0;
@@ -137,6 +173,12 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
     double wheel_base = 2.9;
     double wheelratio = 13.6;
 
+    if(ServiceCarStatus.msysparam.mvehtype == "hunter")
+    {
+        wheel_base = 0.6;
+        wheelratio = 21;
+    }
+
     if(bUseAutowareKappa)
     {
         double kappa = CalcKappa(trace,gpsIndex);

src/decition/decition_brain_sf_changan_shenlan/decition/adc_controller/pid_controller.h

@@ -59,6 +59,8 @@ namespace iv {
 
                     private:
 
+                        double ExtendPreviewDistance(std::vector<Point2D> & trace,double & PreviewDistance);
+
                     };
 
         }

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/compute_00.cpp

@@ -233,7 +233,133 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
         doubledata[j][3]=delta_sum/20;
     }
 }
+namespace iv {
+namespace decition {
+extern double gplanbrakeacc;
+extern double gplanacc;
+}
+}
+//extern double iv::decition::gplanbrakeacc;
+
+void iv::decition::Compute00::SmoothACC(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata)
+{
+    double acca = iv::decition::gplanacc;
+    int nsize = static_cast<int>(vdoubledata.size());
+    int i;
+    for(i=1;i<(nsize-1);i++)
+    {
+        if((vdoubledata[i+1][4] - vdoubledata[i][4])>1.0)
+        {
+            int j;
+            for(j=i;j<(nsize -1);j++)
+            {
+                double fdis = sqrt(pow(MapTrace[j]->gps_x - MapTrace[j+1]->gps_x,2)+pow(MapTrace[j]->gps_y - MapTrace[j+1]->gps_y,2));
+                double vpre = vdoubledata[j][4]/3.6;
+                double fspeed = sqrt((fdis + 0.5*vpre*vpre/acca)*acca/0.5) *3.6;
+                if(fspeed>vdoubledata[j+1][4])
+                {
+                    break;
+                }
+                vdoubledata[j+1][4] = fspeed;
+            }
+            i = j+1;
+        }
+    }
+
+}
+
+
+
+void iv::decition::Compute00::SmoothHighData(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata,unsigned int i)
+{
+    if(i==0)return;
+
+//    0.5*v1*v1/a - 0.5*v2*v2/a = dis;
+//    v1*v1 = (dis + 0.5*v2*v2/a)*a/0.5
+
+    int j;
+    double brakea = 3.0*iv::decition::gplanbrakeacc;//iv::decition::gplanbrakeacc;
+    if(brakea>5.0)brakea = 5.0;
+    int startvalue = static_cast<int>(i-1);
+
+    double fdispoint = sqrt(pow(MapTrace[i]->gps_x - MapTrace[i-1]->gps_x,2)+pow(MapTrace[i]->gps_y - MapTrace[i-1]->gps_y,2));
+    if(fdispoint < 0.1)fdispoint = 0.1;
+    int nprebrake = static_cast<int>(10.0/fdispoint);
+    if((MapTrace[i]->roadMode == 14) || (MapTrace[i]->roadMode == 15))
+    {
+        nprebrake = static_cast<int>(5.0/fdispoint);
+    }
+    else
+    {
+        nprebrake = static_cast<int>(3.0/fdispoint);
+    }
+    for(j=startvalue;j>=0;j--)
+    {
+        if(vdoubledata[j][4] < vdoubledata[j+1][4])
+        {
+            break;
+        }
+        if((startvalue - j) < nprebrake)
+        {
+            vdoubledata[j][4] = vdoubledata[j+1][4];
+            continue;
+        }
+        double fdis = sqrt(pow(MapTrace[j]->gps_x - MapTrace[j+1]->gps_x,2)+pow(MapTrace[j]->gps_y - MapTrace[j+1]->gps_y,2));
+        double vpre = vdoubledata[j+1][4]/3.6;
+        double fspeed = sqrt((fdis + 0.5*vpre*vpre/brakea)*brakea/0.5) *3.6;
+        if(fspeed>vdoubledata[j][4])
+        {
+            break;
+        }
+        vdoubledata[j][4] = fspeed;
+    }
 
+}
+
+void iv::decition::Compute00::SmoothData(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata,unsigned int i)
+{
+    if(i==0)return;
+
+//    0.5*v1*v1/a - 0.5*v2*v2/a = dis;
+//    v1*v1 = (dis + 0.5*v2*v2/a)*a/0.5
+
+    int j;
+    double brakea = iv::decition::gplanbrakeacc;//iv::decition::gplanbrakeacc;
+    int startvalue = static_cast<int>(i-1);
+
+    double fdispoint = sqrt(pow(MapTrace[i]->gps_x - MapTrace[i-1]->gps_x,2)+pow(MapTrace[i]->gps_y - MapTrace[i-1]->gps_y,2));
+    if(fdispoint < 0.1)fdispoint = 0.1;
+    int nprebrake = static_cast<int>(10.0/fdispoint);
+    if((MapTrace[i]->roadMode == 14) || (MapTrace[i]->roadMode == 15))
+    {
+        nprebrake = static_cast<int>(5.0/fdispoint);
+    }
+    else
+    {
+        nprebrake = static_cast<int>(3.0/fdispoint);
+    }
+    for(j=startvalue;j>=0;j--)
+    {
+        if(vdoubledata[j][4] < vdoubledata[j+1][4])
+        {
+            break;
+        }
+        if((startvalue - j) < nprebrake)
+        {
+            vdoubledata[j][4] = vdoubledata[j+1][4];
+            continue;
+        }
+        double fdis = sqrt(pow(MapTrace[j]->gps_x - MapTrace[j+1]->gps_x,2)+pow(MapTrace[j]->gps_y - MapTrace[j+1]->gps_y,2));
+        double vpre = vdoubledata[j+1][4]/3.6;
+        double fspeed = sqrt((fdis + 0.5*vpre*vpre/brakea)*brakea/0.5) *3.6;
+        if(fspeed>vdoubledata[j][4])
+        {
+            break;
+        }
+        vdoubledata[j][4] = fspeed;
+    }
+
+}
 
 /*int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
 {
@@ -358,6 +484,7 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
 }*/
 int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
 {
+    int nSmoothMode = 1;
     int mode0to5count=0,mode0to18count=0,mode0to5countSum=0,mode0to18countSum=0,mode18count=0,mode18countSum=0,mode0to5flash=0,mode18flash=0;
     double straightSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode0,60.0);
     double straightCurveSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode5,40.0);
@@ -366,97 +493,239 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
     getMapDelta(MapTrace);
     for(int i=0;i<doubledata.size();i++)
     {
-        if((MapTrace[i]->mfCurvature>-0.02)&&(MapTrace[i]->mfCurvature<0.02)){
-            MapTrace[i]->roadMode=5;
-        }else if(((MapTrace[i]->mfCurvature>=0.02)&&(MapTrace[i]->mfCurvature<=0.16))||((MapTrace[i]->mfCurvature>=-0.16)&&(MapTrace[i]->mfCurvature<=-0.02))){
-            MapTrace[i]->roadMode=18;
-        }else if(((MapTrace[i]->mfCurvature>0.16))||((MapTrace[i]->mfCurvature<-0.16))){
-            MapTrace[i]->roadMode=14;
+
+        if((MapTrace[i]->roadMode==14)||(MapTrace[i]->roadMode==15))
+        {
+           int  a = 1;
+        }
+        else
+        {
+            if((MapTrace[i]->mfCurvature>-0.02)&&(MapTrace[i]->mfCurvature<0.02)){
+                MapTrace[i]->roadMode=5;
+            }else if(((MapTrace[i]->mfCurvature>=0.02)&&(MapTrace[i]->mfCurvature<=0.16))||((MapTrace[i]->mfCurvature>=-0.16)&&(MapTrace[i]->mfCurvature<=-0.02))){
+                MapTrace[i]->roadMode=18;
+            }else if(((MapTrace[i]->mfCurvature>0.16))||((MapTrace[i]->mfCurvature<-0.16))){
+                MapTrace[i]->roadMode=14;
+            }
         }
     }
-                for(int i=0;i<MapTrace.size();i++)
-                {
-                    if(MapTrace[i]->roadMode==0){
-                        doubledata[i][4]=straightSpeed;
-                        mode0to5count++;
-                        //mode0to18count++;
-                        mode18count=0;
-                        mode0to5flash=mode0to5count;
-                    }else if(MapTrace[i]->roadMode==5){
-                        doubledata[i][4]=straightCurveSpeed;
-                        mode0to5count++;
-                        //mode0to18count++;
-                        mode18count=0;
-                        mode0to5flash=mode0to5count;
-                    }else if(MapTrace[i]->roadMode==18){
-                        mode0to5countSum=mode0to5count;
-                        doubledata[i][4]=Curve_SmallSpeed;
 
-                        double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*10/0.6;
-                        int brake_distance=(int)brake_distance_double;
-                        int step_count=0;
-                        if((i>brake_distance)&&(mode0to5countSum>brake_distance))
-                        {
-                            double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;
+    for(int i=1;i<MapTrace.size()-1;i++)
+    {
+         if((MapTrace[i-1]->roadMode==5)&&(MapTrace[i+1]->roadMode==5))
+         {
+          if(MapTrace[i]->roadMode!=5)
+           MapTrace[i]->roadMode=5;
+
+         }
+         else if ((MapTrace[i-1]->roadMode==18)&&(MapTrace[i+1]->roadMode==18))
+         {
+          if(MapTrace[i]->roadMode==5)
+           MapTrace[i]->roadMode=18;
+          if(MapTrace[i]->roadMode==14)
+           MapTrace[i]->roadMode=18;
+         }
+            else if ((MapTrace[i-1]->roadMode==14)&&(MapTrace[i+1]->roadMode==14))
+         {
+          if(MapTrace[i]->roadMode==5)
+           MapTrace[i]->roadMode=14;
+                 if(MapTrace[i]->roadMode==18)
+           MapTrace[i]->roadMode=14;
+         }
+         else
+         {
+
+         }
+    }
+    if(nSmoothMode == 0)
+        {
 
-                            for(int j=i;j>i-brake_distance;j--){
-                                doubledata[j][4]=min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
-                                step_count++;
+                    for(int i=0;i<MapTrace.size();i++)
+                    {
+                        if(MapTrace[i]->roadMode==0){
+                            doubledata[i][4]=straightSpeed;
+                            if(MapTrace[i]->speed>0)
+                            {
+         //                       double fSpeed = MapTrace[i]->speed;
+                                doubledata[i][4] = MapTrace[i]->speed*3.6;
                             }
-                        }else if(mode0to5countSum>0){
-                            for(int j=i;j>=i-mode0to5countSum;j--){
-                                doubledata[j][4]=Curve_SmallSpeed;
-                                step_count++;
+                            mode0to5count++;
+                            //mode0to18count++;
+                            mode18count=0;
+                            mode0to5flash=mode0to5count;
+                        }else if(MapTrace[i]->roadMode==5){
+                            doubledata[i][4]=straightCurveSpeed;
+                            if(MapTrace[i]->speed>0)
+                            {
+         //                       double fSpeed = MapTrace[i]->speed;
+                                doubledata[i][4] = MapTrace[i]->speed*3.6;
                             }
-                        }else{
+                            mode0to5count++;
+                            //mode0to18count++;
+                            mode18count=0;
+                            mode0to5flash=mode0to5count;
+                        }else if(MapTrace[i]->roadMode==18){
+                            mode0to5countSum=mode0to5count;
                             doubledata[i][4]=Curve_SmallSpeed;
-                        }
-                        mode0to5count=0;
-                        //mode0to18count++;
-                        mode18count++;
-                        mode18flash=mode18count;
-                    }else if(MapTrace[i]->roadMode==14){
-                        mode0to18countSum=mode0to5flash+mode18flash;
-                        mode18countSum=mode18count;
-                        double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
-                        double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
-                        int brake_distanceLarge=(int)brake_distanceLarge_double;
-                        int brake_distance0to18=(int)brake_distance0to18_double;
-                        int step_count=0;
-                        doubledata[i][4]=Curve_LargeSpeed;
 
-                        if(mode18countSum>brake_distanceLarge)
-                        {
-                                double step_speed=(Curve_SmallSpeed-Curve_LargeSpeed)/brake_distanceLarge;
+                            double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*5/0.6; //1228
+                            int brake_distance=(int)brake_distance_double;
+                            int step_count=0;
+                            if((i>brake_distance)&&(mode0to5countSum>brake_distance))
+                            {
+                                double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;
+
+                                for(int j=i;j>i-brake_distance;j--){
+                                    doubledata[j][4]=min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
 
-                                for(int j=i;j>i-brake_distanceLarge;j--){
-                                    doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),Curve_SmallSpeed);
                                     step_count++;
                                 }
-
-                        }else if(mode0to18countSum>brake_distance0to18){
-
-                                double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;
-
-                                for(int j=i;j>i-brake_distance0to18;j--){
-                                    doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
+                            }else if(mode0to5countSum>0){
+                                double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;//1228
+                                for(int j=i;j>=i-mode0to5countSum;j--){
+                                    doubledata[j][4]=Curve_SmallSpeed;//min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);//1228Curve_SmallSpeed;
                                     step_count++;
                                 }
-                        }else if(mode0to18countSum>0){
-                            for(int j=i;j>=i-mode0to18countSum;j--){
-                                doubledata[j][4]=Curve_LargeSpeed;
-                                step_count++;
+                            }else{
+                                doubledata[i][4]=Curve_SmallSpeed;
                             }
-                        }else{
-                                doubledata[i][4]=Curve_LargeSpeed;
+                            mode0to5count=0;
+                            //mode0to18count++;
+                            mode18count++;
+                            mode18flash=mode18count;
+                        }else if((MapTrace[i]->roadMode==14)||(MapTrace[i]->roadMode==15)){
+                            mode0to18countSum=mode0to5flash+mode18flash;
+                            mode18countSum=mode18count;
+                            double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*5/0.6; //1228
+                            double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*5/0.6;
+                            int brake_distanceLarge=(int)brake_distanceLarge_double;
+                            int brake_distance0to18=(int)brake_distance0to18_double;
+                            int step_count=0;
+                            doubledata[i][4]=Curve_LargeSpeed;
+
+                            if(mode18countSum>brake_distanceLarge)
+                            {
+                                    double step_speed=(Curve_SmallSpeed-Curve_LargeSpeed)/brake_distanceLarge;
+
+                                    for(int j=i;j>i-brake_distanceLarge;j--){
+                                        doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),Curve_SmallSpeed);
+                                        step_count++;
+                                    }
+
+                            }else if(mode0to18countSum>brake_distance0to18){
+
+                                    double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;
+
+                                    for(int j=i;j>i-brake_distance0to18;j--){
+                                        if(MapTrace[j]->roadMode==18)//20230106,fangzhi 18moshi de sudu dayu yuzhi
+                                            continue;
+                                        doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
+                                        step_count++;
+                                    }
+                            }else if(mode0to18countSum>0){
+                                double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;//1228
+                                for(int j=i;j>=i-mode0to18countSum;j--){
+                                    doubledata[j][4]=Curve_LargeSpeed;//min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);//1228Curve_LargeSpeed;
+                                    step_count++;
+                                }
+                            }else{
+                                    doubledata[i][4]=Curve_LargeSpeed;
+                            }
+
+                            mode0to5count=0;
+                            mode18count=0;
+                            mode0to5flash=0;
+                            mode18flash=0;
                         }
+                    }
 
-                        mode0to5count=0;
-                        mode18count=0;
-                        mode0to5flash=0;
-                        mode18flash=0;
+
+        }
+    else
+    {
+        for(int i=0;i<MapTrace.size();i++)
+        {
+            if(MapTrace[i]->roadMode==0){
+                doubledata[i][4]=straightSpeed;
+                if(MapTrace[i]->speed>0)
+                {
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
+
+                }
+
+
+                if(i>0)
+                {
+                    if((doubledata[i-1][4] - doubledata[i][4]) > 5.0)
+                    {
+                        SmoothHighData(MapTrace,doubledata,i);
+                    }
+                }
+
+            }else if(MapTrace[i]->roadMode==5){
+                doubledata[i][4]=straightCurveSpeed;
+                if(MapTrace[i]->speed>0)
+                {
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
+
+                }
+
+
+                if(i>0)
+                {
+                    if((doubledata[i-1][4] - doubledata[i][4]) > 5.0)
+                    {
+                        SmoothHighData(MapTrace,doubledata,i);
                     }
                 }
+
+            }else if(MapTrace[i]->roadMode==18){
+
+                if(fabs(MapTrace[i]->mfCurvature)>0.06)
+                {
+                    doubledata[i][4]=(Curve_SmallSpeed + Curve_LargeSpeed)/2.0;
+                }
+                else
+                    doubledata[i][4]=Curve_SmallSpeed;
+
+                if((MapTrace[i]->speed>0) &&(doubledata[i][4]>(MapTrace[i]->speed*3.6)))
+                {
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
+                }
+                if(i>0)
+                {
+                    if(MapTrace[i-1]->roadMode != MapTrace[i]->roadMode)
+                    {
+                        SmoothData(MapTrace,doubledata,i);
+                    }
+                }
+
+
+            }else if((MapTrace[i]->roadMode==14)||(MapTrace[i]->roadMode==15)){
+
+                doubledata[i][4]=Curve_LargeSpeed;
+                if((MapTrace[i]->speed>0) &&(doubledata[i][4]>(MapTrace[i]->speed*3.6)))
+                {
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
+                }
+                if(i>0)
+                {
+                    if(MapTrace[i-1]->roadMode != MapTrace[i]->roadMode)
+                    {
+                        SmoothData(MapTrace,doubledata,i);
+                    }
+                }
+
+            }
+        }
+
+        SmoothACC(MapTrace,doubledata);
+    }
+
 //
                 /*for(int i=0;i<MapTrace.size();i++){
                     //将数据写入文件开始

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/compute_00.h

@@ -37,6 +37,10 @@ namespace iv {
             static int getDesiredSpeed(std::vector<GPSData> gpsMap);
             static int getMapDelta(std::vector<GPSData> MapTrace);
             static int getPlanSpeed(std::vector<GPSData> MapTrace);
+            static void SmoothACC(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata);
+            static void SmoothData(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata,unsigned int i);
+
+            static void SmoothHighData(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata,unsigned int i);
 
 
             static double getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed);

src/decition/decition_brain_sf_changan_shenlan/decition/brain.cpp

@@ -28,6 +28,8 @@ extern std::string gstrmembraincarstate;
 namespace iv {
 namespace decition {
         iv::decition::BrainDecition * gbrain;
+        double gplanbrakeacc=0.3;
+        double gplanacc=1.0;
 
         void ListenTraceMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
         {
@@ -374,10 +376,25 @@ void iv::decition::BrainDecition::run() {
     std::string brkdebug = xp.GetParam("brakeDebug","3");
 
 
+
     ServiceCarStatus.ang_debug = atof(angdebug.data());
     ServiceCarStatus.torque_or_acc = atof(tordebug.data());
     ServiceCarStatus.brake = atof(brkdebug.data());
 
+    std::string strbasepitch = xp.GetParam("basepitch","0.0");
+    ServiceCarStatus.mfbasepitch = atof(strbasepitch.data());
+
+    ServiceCarStatus.mbUseRampAssit = xp.GetParam("UseRamp",false);
+
+    gplanbrakeacc = fabs(xp.GetParam("planbrakeacc",0.3));
+    if(gplanbrakeacc <= 0.01)gplanbrakeacc = 0.01;
+
+    gplanacc = fabs(xp.GetParam("planacc",1.0));
+    if(gplanacc <0.1)gplanacc = 0.1;
+
+
+    decitionGps00->mstopbrake = fabs(xp.GetParam("stopbrake",0.6));
+    if(decitionGps00->mstopbrake < 0.1)decitionGps00->mstopbrake = 0.1;
 
     mstrmemmap_index = xp.GetParam("msg_mapindex","map_index");
 
@@ -496,6 +513,8 @@ void iv::decition::BrainDecition::run() {
         eyes->getSensorObstacle(obs_radar_, obs_camera_,gps_data_, obs_lidar_grid_);	//从传感器线程临界区交换数据
 
 
+
+
         ServiceLidar.copylidarperto(lidar_per);
 
 

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -28,7 +28,7 @@ extern iv::Ivlog * givlog;
 #define AVOID_NEW 1
 iv::decition::DecideGps00::DecideGps00()
 {
-
+    std::cout<<" init gps00"<<std::endl;
 }
 iv::decition::DecideGps00::~DecideGps00()
 {
@@ -188,7 +188,7 @@ std::vector<std::vector<double>> doubledata;//大地坐标系下x,y,phi,delta，
 
 enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
                 waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
-                dRever,dRever0,dRever1,dRever2,dRever3,dRever4} vehState,lastVehState;
+                dRever,dRever0,dRever1,dRever2,dRever3,dRever4,dReverTZD,dReverTZDing} vehState,lastVehState;
 
 
 std::vector<iv::Point2D>  gpsTraceAim; //记录变道后的目标路径，若车辆经frenet规划行驶至该路径上后，就关闭frenet规划
@@ -347,6 +347,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     ServiceCarStatus.mavoidX=avoidXNew;
 
 
+
+    if(front_car_decide_avoid == false)    //geely logic yizhi
+    {
+        obstacle_avoid_flag =  0;
+    }
+
+
     if(ServiceCarStatus.msysparam.gpsOffset_y!=0 || ServiceCarStatus.msysparam.gpsOffset_x!=0){
         GPS_INS gpsOffset = Coordinate_UnTransfer(ServiceCarStatus.msysparam.gpsOffset_x, ServiceCarStatus.msysparam.gpsOffset_y, now_gps_ins);
         now_gps_ins.gps_x=gpsOffset.gps_x;
@@ -417,6 +424,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     secSpeed = realspeed / 3.6;
 
 
+    if(ServiceCarStatus.mnParkType==2)//guosai pingtai yanzheng ceting
+    {
+        if(ServiceCarStatus.bocheMode &&(vehState != dReverTZD))
+        {
+            vehState = dReverTZD;
+        }
+    }
 
 
     //sidePark
@@ -438,6 +452,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             ServiceCarStatus.bocheEnable=2;
         }
 
+
+
         if(ServiceCarStatus.bocheMode && vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 && vehState!=dRever2
                 &&  vehState!=dRever3 && vehState!=dRever4 &&  vehState!=reverseArr )
         {
@@ -503,6 +519,41 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
     }
 
+    if(ServiceCarStatus.mnParkType == 2)
+        ServiceCarStatus.bocheEnable=1;
+
+
+    if(vehState == dReverTZD)
+    {
+         Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+         if(pt.y<0.5)
+         {
+             if(abs(realspeed)>0.1)
+             {
+                 vehState = reverseArr;
+             }
+             gps_decition->wheel_angle = 0;
+             gps_decition->speed = dSpeed;
+             obsDistance=-1;
+             minDecelerate=min(minDecelerate,-0.5f);
+             phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+             //	gps_decition->accelerator = 0;
+             return gps_decition;
+         }
+         else
+         {
+             gps_decition->wheel_angle = 0;
+             gps_decition->speed = dSpeed;
+             obsDistance=-1;
+             dSpeed = 2;
+             dSecSpeed = dSpeed / 3.6;
+             gps_decition->speed = dSpeed;
+             phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+             //	gps_decition->accelerator = 0;
+             return gps_decition;
+         }
+    }
+
     // ChuiZhiTingChe
     if (vehState == reverseCar)
     {
@@ -513,7 +564,12 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if (vehState == reversing)
     {
         double dis = GetDistance(now_gps_ins, Compute00().nearTpoint);
-        if (dis<2.0)//0.5
+        iv::Point2D pt = iv::decition::Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y,aim_gps_ins);
+        iv::Point2D ptnear = iv::decition::Coordinate_Transfer(Compute00().nearTpoint.gps_x,Compute00().nearTpoint.gps_y,aim_gps_ins);
+
+         double fdistonear = fabs(pt.x - ptnear.x);
+         if(fdistonear<0.5)  //将吉利项目中的停车逻辑移植过来
+//        if (dis<2.0)//0.5
         {
             vehState = reverseCircle;
             qiedianCount = true;
@@ -964,7 +1020,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 //        }
 //    }
     int useaccend = 1;
-    double mstopbrake=0.3;
+    double mstopbrake=0.6;
     if(((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))||(useaccend == 1))
     {
                 if(PathPoint+1000>gpsMapLine.size()){
@@ -973,6 +1029,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                     distoend=1000;
                 }
                 if(!circleMode && distoend<100){
+                        if(distoend<3.0)
+                        {
+                            std::cout<<"distoend:  "<<distoend<<std::endl;
+                        }
                         double nowspeed = realspeed/3.6;
                         double brakedis = 100;
                         double stopbrake = mstopbrake;
@@ -989,9 +1049,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                             if(distoend == 0.0)distoend = 0.09;
                             acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
                             if((acc_end<(-3.0))&&(nowspeed<10))acc_end = -3.0;
+                            dSecSpeed = sqrt(2.0 * fabs(acc_end) * distoend);
+                            dSpeed = dSecSpeed * 3.6;
                         }
 
-                        if((distoend < 1.2)||(PathPoint>=gpsMapLine.size()-2))acc_end = (stopbrake + 0.1)*(-1.0);
+                        if((distoend <= 0.1)||(PathPoint>=gpsMapLine.size()-2))acc_end = (stopbrake + 0.1)*(-1.0);
 
 
                         if(acc_end<0)minDecelerate = acc_end;
@@ -1277,17 +1339,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
                 getGpsTraceNowLeftRight(gpsTraceNow);
             }
-            else //if((vehState==preAvoid)||(vehState==normalRun)||(vehState))
+            else
             {
-                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
-                gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
-                gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
+                gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidXNew);
+                getGpsTraceNowLeftRight(gpsTraceNow);
             }
-//            else
-//            {
-//                gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidXNew);
-//                getGpsTraceNowLeftRight(gpsTraceNow);
-//            }
         }
     }
 #else
@@ -1561,6 +1617,21 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
     }
 
+    if(now_gps_ins.rtk_status == 16)
+    {
+        double sugSpeed = realspeed - 2.0;
+        if(sugSpeed<=5.0)sugSpeed = 5.0;
+        dSpeed = min(sugSpeed,dSpeed);
+
+    }
+    if(now_gps_ins.rtk_status == 15)
+    {
+        double sugSpeed = realspeed - 2.0;
+        if(sugSpeed<=2.0)sugSpeed = 5.0;
+        dSpeed = min(sugSpeed,dSpeed);
+
+    }
+
     if (gpsMapLine[PathPoint]->speed_mode == 2)
     {
         dSpeed = min(25.0,dSpeed);
@@ -1820,7 +1891,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         {
             vehState=normalRun;
             obstacle_avoid_flag=0;
-            avoidXNewPreVector.clear();
+           // avoidXNewPreVector.clear();//20230526,huifu
         }
     }
 
@@ -1902,13 +1973,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
     else
     {
-        if(PathPoint+400<gpsMapLine.size()){
+        if(PathPoint+200<gpsMapLine.size()){   //400gaiwei 200
             int road_permit_sum=0;
-            for(int k=PathPoint;k<PathPoint+400;k++){
+            for(int k=PathPoint;k<PathPoint+200;k++){
                 if((gpsMapLine[k]->mbnoavoid==false))//&&(gpsMapLine[k]->roadSum>1))
                     road_permit_sum++;
             }
-            if(road_permit_sum>=400)
+            if(road_permit_sum>=200)
                 road_permit_avoid=true;
         }
     }
@@ -2015,118 +2086,156 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
         givlog->debug("decition_brain","======avoidXNewPre==========:%f",avoidXNewPre);
 
-        const int nAvoidPreCount = 100;
+        //const int nAvoidPreCount = 100;
+        if(avoidXNewPreVector.size()<5){
+                   avoidXNewPreVector.push_back(avoidXNewPre);
+               }else{
+                   if(avoidXNewPreVector[0]!=avoidXNewLast){
+                       for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
+                           if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
+                               avoidXNewPreFilter=avoidXNewLast;
+                               break;
+                           }
+                           if(i==avoidXNewPreVector.size()-1)
+                               avoidXNewPreFilter=avoidXNewPreVector[0];
+                       }
+                   }
+                   avoidXNewPreVector.clear();
+               }
+               if(avoidXNewPreFilter!=avoidXNewLast){
+                   avoidXNew=avoidXNewPre;
+                   if(avoidXNew<0)
+                       turnLampFlag=-1;
+                   else if(avoidXNew>0)
+                       turnLampFlag=1;
+                   else
+                       turnLampFlag=0;
+
+                   gpsTraceNow.clear();
+                   gpsTraceAvoidXY.clear();
+                   givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
+                                 avoidXNew,avoidXNewLast);
+                   //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+                   gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+                   vehState = avoiding;
+                   startAvoidGpsIns = now_gps_ins;
+                   obstacle_avoid_flag=1;
+                   hasCheckedAvoidLidar = false;
+                   avoidXNewLast=avoidXNew;
+                   avoidXNewPreFilter=avoidXNew;
+               }
+           }
 
-        if(avoidXNewPreVector.size()<nAvoidPreCount){
-            avoidXNewPreVector.push_back(avoidXNewPre);
-        }
-        else
-        {
-            avoidXNewPreVector.erase(avoidXNewPreVector.begin());
-            avoidXNewPreVector.push_back(avoidXNewPre);
-        }
+//        if(avoidXNewPreVector.size()<nAvoidPreCount){
+//            avoidXNewPreVector.push_back(avoidXNewPre);
+//        }
+//        else
+//        {
+//            avoidXNewPreVector.erase(avoidXNewPreVector.begin());
+//            avoidXNewPreVector.push_back(avoidXNewPre);
+//        }
 
-        if(vehState == preAvoid)
-        {
-            avoidXNew = avoidXNewPre;
-        }
-        else
-        {
-            if(fabs(avoidXNewLast)>0.1)
-            {
+//        if(vehState == preAvoid)
+//        {
+//            avoidXNew = avoidXNewPre;
+//        }
+//        else
+//        {
+//            if(fabs(avoidXNewLast)>0.1)
+//            {
 
-                bool bOriginSafe = true;
+//                bool bOriginSafe = true;
 
-                if(avoidXNewPreVector.size()<nAvoidPreCount)
-                {
-                    bOriginSafe = false;
-                }
-                else
-                {
-                    unsigned int j;
-                    unsigned int nSize = avoidXNewPreVector.size();
-                    for(j=0;j<nSize;j++)
-                    {
-                        if(fabs(avoidXNewPreVector[j])>0.1)
-                        {
-                            bOriginSafe = false;
-                            break;
-                        }
-                    }
-                }
-                if(bOriginSafe)
-                {
-                    avoidXNew = 0;
- //                   avoidXNewPreVector.clear();
-                }
-                else
-                {
-                    unsigned int j;
-                    unsigned int nSize = avoidXNewPreVector.size();
-                    if(avoidXNewPreVector.size()==nAvoidPreCount)
-                    {
-                        double filter = 0;
-                        for(j=0;j<nSize;j++)
-                        {
-                            if(fabs(avoidXNewPreVector[j])>0.1)
-                            {
-                                if(fabs(filter)<0.1)filter = avoidXNewPreVector[j];
-                                else
-                                {
-                                    if(fabs(filter - avoidXNewPreVector[j])>0.1)
-                                    {
-                                        filter = 0;
-                                        break;
-                                    }
-                                }
-                            }
-                        }
-                        if(fabs(filter)<0.1)
-                        {
-                            avoidXNew = avoidXNewLast;
-                        }
-                        else
-                        {
-                            if(fabs(filter - avoidXNewLast)>=2.0)
-                            {
-                                avoidXNew = filter;
-                            }
-                            else
-                            {
-                                avoidXNew = avoidXNewLast;
-                            }
-                        }
-                    }
-                    else
-                    {
-                        avoidXNew = avoidXNewLast;
-                    }
-                }
-            }
-        }
+//                if(avoidXNewPreVector.size()<nAvoidPreCount)
+//                {
+//                    bOriginSafe = false;
+//                }
+//                else
+//                {
+//                    unsigned int j;
+//                    unsigned int nSize = avoidXNewPreVector.size();
+//                    for(j=0;j<nSize;j++)
+//                    {
+//                        if(fabs(avoidXNewPreVector[j])>0.1)
+//                        {
+//                            bOriginSafe = false;
+//                            break;
+//                        }
+//                    }
+//                }
+//                if(bOriginSafe)
+//                {
+//                    avoidXNew = 0;
+// //                   avoidXNewPreVector.clear();
+//                }
+//                else
+//                {
+//                    unsigned int j;
+//                    unsigned int nSize = avoidXNewPreVector.size();
+//                    if(avoidXNewPreVector.size()==nAvoidPreCount)
+//                    {
+//                        double filter = 0;
+//                        for(j=0;j<nSize;j++)
+//                        {
+//                            if(fabs(avoidXNewPreVector[j])>0.1)
+//                            {
+//                                if(fabs(filter)<0.1)filter = avoidXNewPreVector[j];
+//                                else
+//                                {
+//                                    if(fabs(filter - avoidXNewPreVector[j])>0.1)
+//                                    {
+//                                        filter = 0;
+//                                        break;
+//                                    }
+//                                }
+//                            }
+//                        }
+//                        if(fabs(filter)<0.1)
+//                        {
+//                            avoidXNew = avoidXNewLast;
+//                        }
+//                        else
+//                        {
+//                            if(fabs(filter - avoidXNewLast)>=2.0)
+//                            {
+//                                avoidXNew = filter;
+//                            }
+//                            else
+//                            {
+//                                avoidXNew = avoidXNewLast;
+//                            }
+//                        }
+//                    }
+//                    else
+//                    {
+//                        avoidXNew = avoidXNewLast;
+//                    }
+//                }
+//            }
+//        }
 
 
-        if(avoidXNew<0)
-            turnLampFlag=-1;
-        else if(avoidXNew>0)
-            turnLampFlag=1;
-        else
-            turnLampFlag=0;
+//        if(avoidXNew<0)
+//            turnLampFlag=-1;
+//        else if(avoidXNew>0)
+//            turnLampFlag=1;
+//        else
+//            turnLampFlag=0;
 
-        if(fabs(avoidXNew - avoidXNewLast)>0.1)
-        {
-            gpsTraceNow.clear();
-            gpsTraceAvoidXY.clear();
-            givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
-                          avoidXNew,avoidXNewLast);
-            //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
-            gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
-            vehState = avoiding;
-            startAvoidGpsIns = now_gps_ins;
-            obstacle_avoid_flag=1;
-            hasCheckedAvoidLidar = false;
-            avoidXNewLast=avoidXNew;
-        }
+//        if(fabs(avoidXNew - avoidXNewLast)>0.1)
+//        {
+//            gpsTraceNow.clear();
+//            gpsTraceAvoidXY.clear();
+//            givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
+//                          avoidXNew,avoidXNewLast);
+//            //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+//            gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+//            vehState = avoiding;
+//            startAvoidGpsIns = now_gps_ins;
+//            obstacle_avoid_flag=1;
+//            hasCheckedAvoidLidar = false;
+//            avoidXNewLast=avoidXNew;
+//        }
 
 //        if(avoidXNewPreVector.size()<5){
 //            avoidXNewPreVector.push_back(avoidXNewPre);
@@ -2165,7 +2274,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 //            avoidXNewLast=avoidXNew;
 //            avoidXNewPreFilter=avoidXNew;
 //        }
-    }
+//    }
 //20230407,
 //    if((vehState == preAvoid)||((avoidXNew!=0)))
 //  //    if((vehState == preAvoid)||(avoidXNew!=0))
@@ -2673,9 +2782,17 @@ else
 
     if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
     {
-        if(dSpeed>10.0)
+        if(dSpeed>20.0)
         {
-            dSpeed=10.0;   //shenlan bisai xiansu 10
+            dSpeed=20.0;   //shenlan bisai xiansu 10
+        }
+    }
+
+    if(ServiceCarStatus.msysparam.mvehtype=="hunter")
+    {
+        if(dSpeed>5.0)
+        {
+            dSpeed=5.0;
         }
     }
 
@@ -2862,6 +2979,8 @@ else
                          else
                              obsDistance_log=obsDistance;
                          QDateTime dt2=QDateTime::currentDateTime();
+                         double disToEndTrace =  GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
+                         double disToParkPoint =  GetDistance(now_gps_ins, aim_gps_ins);
                          outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
                                  <<"Decide_Dspeed"  << ","  <<setprecision(2)<<dSpeed<< ","
                                  <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
@@ -2869,6 +2988,8 @@ else
                                  <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
   //                             <<"readyParkMode"<< ","<<readyParkMode<< ","
   //                             <<"readyZhucheMode"<< ","<<readyZhucheMode<< ","
+                                 <<"trace_disToEndTrace"<< ","<<disToEndTrace<< ","
+                                 <<"disToParkPoint"<< ","<<disToParkPoint<< ","
                                  <<"OBS_Distance"<< ","<<obsDistance_log<< ","
                                  <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
                                  <<"obsSpeed_fusion"<<","<<obsSpeed<<","
@@ -2889,6 +3010,12 @@ else
                                  <<"Vehicle_GPS_lng"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
                                  <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_x<< ","
                                  <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_y<< ","
+
+                                <<"aim_GPS_lat"<< ","<< setprecision(10)<<aim_gps_ins.gps_lat<< ","
+                                <<"aim_GPS_lng"<< ","<< setprecision(10)<<aim_gps_ins.gps_lng<< ","
+                                <<"aim_GPS_X"<< ","<< setprecision(10)<<aim_gps_ins.gps_x<< ","
+                                <<"aim_GPS_Y"<< ","<< setprecision(10)<<aim_gps_ins.gps_y<< ","
+
                                  <<"MAP_GPS_heading"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->ins_heading_angle<< ","
                                  <<"MAP_GPS_X"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_x<< ","
                                  <<"MAP_GPS_Y"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_y<< ","
@@ -2944,6 +3071,7 @@ void iv::decition::DecideGps00::initMethods(){
     hapo_Adapter = new HapoAdapter();
     sightseeing_Adapter = new SightseeingAdapter();
     changanshenlan_Adapter = new ChanganShenlanAdapter();
+    hunter_Adapter = new HunterAdapter();
 
 
     mpid_Controller.reset(pid_Controller);
@@ -2955,6 +3083,7 @@ void iv::decition::DecideGps00::initMethods(){
     mhapo_Adapter.reset(hapo_Adapter);
     msightseeing_Adapter.reset(sightseeing_Adapter);
     mchanganshenlan_Adapter.reset(changanshenlan_Adapter);
+    mhunter_Adapter.reset(hunter_Adapter);
 
     frenetPlanner = new FrenetPlanner();
     //    laneChangePlanner = new LaneChangePlanner();
@@ -2995,6 +3124,9 @@ void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decit
     else if(ServiceCarStatus.msysparam.mvehtype=="shenlan"){
         changanshenlan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
     }
+    else if(ServiceCarStatus.msysparam.mvehtype=="hunter"){
+        hunter_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }
 }
 
 
@@ -3007,13 +3139,18 @@ std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_
     int tracesize=800;
     if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
     {
-        tracesize=400;
+        tracesize=150;//400;
     }
     else
     {
         tracesize=800;
     }
 
+    if(ServiceCarStatus.msysparam.mvehtype=="hunter")
+    {
+        tracesize=100;//400;
+    }
+
     if (gpsMapLine.size() > tracesize && PathPoint >= 0)
     {
         int aimIndex;
@@ -4781,18 +4918,40 @@ float iv::decition::DecideGps00::ComputeV2XTrafficLightSpeed(iv::TrafficLight tr
         traffic_time = trafficLight.straightTime;
     }
 
-    passThrough = computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,dSecSpeed);
-    if(passThrough)
-    {
+//    passThrough = computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,dSecSpeed);
+//    if(passThrough)
+//    {
+//        return trafficSpeed;
+//    }
+//    else
+//    {
+//        trafficSpeed = computeTrafficSpeedLimt(nearTrafficDis);
+//        if(nearTrafficDis < 6)
+//        {
+//            float decelerate = 0 - ( secSpeed * secSpeed * 0.5 / nearTrafficDis);
+//            minDecelerate = min(minDecelerate,decelerate);
+//        }
+//        return trafficSpeed;
+//    }
+    double fTrafficBrake = 1.0;  //移植吉利项目的v2x
+    passThrough=computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,secSpeed,dSecSpeed);
+    if(passThrough){
         return trafficSpeed;
-    }
-    else
-    {
-        trafficSpeed = computeTrafficSpeedLimt(nearTrafficDis);
-        if(nearTrafficDis < 6)
-        {
-            float decelerate = 0 - ( secSpeed * secSpeed * 0.5 / nearTrafficDis);
-            minDecelerate = min(minDecelerate,decelerate);
+    }else{
+        trafficSpeed=computeTrafficSpeedLimt(nearTrafficDis);
+        if(nearTrafficDis<(dSecSpeed*dSecSpeed/(2*fTrafficBrake))){
+            if(fabs(nearTrafficDis)<1.0)
+            {
+                minDecelerate = -1.0 * fTrafficBrake;
+            }
+            else
+            {
+                if((0.5*secSpeed*secSpeed/fTrafficBrake) > (nearTrafficDis*0.9))
+                {
+                    float decelerate =0-( secSpeed*secSpeed*0.5/(nearTrafficDis-0.5));
+                    minDecelerate=min(minDecelerate,decelerate);
+                }
+            }
         }
         return trafficSpeed;
     }
@@ -4800,9 +4959,12 @@ float iv::decition::DecideGps00::ComputeV2XTrafficLightSpeed(iv::TrafficLight tr
     return trafficSpeed;
 }
 
-bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float dSecSpeed)
+//bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float dSecSpeed)
+bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed)
 {
     float passTime=0;
+    double fAcc = 0.6;
+    if(dSecSpeed == 0)return true;
     if (trafficColor == 2 || trafficColor == 3)
     {
         return false;
@@ -4816,6 +4978,16 @@ bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficC
         passTime=trafficDis/dSecSpeed;
         if(passTime+1< trafficTime)
         {
+            double passTime2= trafficTime - 0.1;
+            double disthru = realSecSpeed * passTime2 + 0.5 * fAcc * pow(passTime2,2);
+            if((disthru -1.0)< trafficDis )
+            {
+                if((realSecSpeed<3.0)&&(trafficDis>10))
+                {
+                    return true;
+                }
+                return false;
+            }
             return true;
         }
         else
@@ -4828,6 +5000,16 @@ bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficC
 float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis)
 {
     float limit=200;
+    double fBrake = 1.0;
+    if(trafficDis < 1.0)
+    {
+        limit =0;
+    }
+    else
+    {
+        limit = 3.6*sqrt(2.0* fabs(fBrake) * (trafficDis-1.0) );
+    }
+    return limit;
     if(trafficDis<10)
     {
         limit = 0;

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.h

@@ -17,6 +17,7 @@
 #include <decition/adc_adapter/hapo_adapter.h>
 #include <decition/adc_adapter/changan_shenlan_adapter.h>
 #include <decition/adc_adapter/sightseeing_adapter.h>
+#include <decition/adc_adapter/hunter_adapter.h>
 #include "perception/perceptionoutput.h"
 #include "ivlog.h"
 #include <memory>
@@ -157,6 +158,7 @@ public:
     BaseAdapter *hapo_Adapter;
     BaseAdapter *sightseeing_Adapter;
     BaseAdapter *changanshenlan_Adapter;
+    BaseAdapter *hunter_Adapter;
 
     std::shared_ptr<BaseController> mpid_Controller;
     std::shared_ptr<BaseAdapter> mge3_Adapter;
@@ -167,6 +169,7 @@ public:
     std::shared_ptr<BaseAdapter> mhapo_Adapter;
     std::shared_ptr<BaseAdapter> msightseeing_Adapter;
     std::shared_ptr<BaseAdapter> mchanganshenlan_Adapter;
+    std::shared_ptr<BaseAdapter> mhunter_Adapter;
 
 	FrenetPlanner *frenetPlanner;
     SplinePlanner *splinePlanner;
@@ -176,6 +179,9 @@ public:
 //    std::shared_ptr<BasePlanner> mlaneChangePlanner;
 
 
+    double mstopbrake = 0.6;
+
+
     Decition getDecideFromGPS(GPS_INS gpsIns, const std::vector<GPSData> navigation_data, iv::LidarGridPtr lidarGridPtr,
                               std::vector<iv::Perception::PerceptionOutput> lidar_per,iv::TrafficLight trafficLight);
 
@@ -241,7 +247,8 @@ public:
                                    int pathpoint,float secSpeed,float dSpeed,bool circleMode);
     float getTrafficLightSpeed(int traffic_light_color, int traffic_light_time, float dis);
 
-    bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float dSecSpeed);
+    //bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float dSecSpeed);
+    bool computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed);
     float computeTrafficSpeedLimt(float trafficDis);
     float ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
                                                                int pathpoint,float secSpeed,float dSpeed);

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00_copy.cpp

@@ -0,0 +1,5406 @@
+#include <decition/decide_gps_00.h>
+#include <decition/adc_tools/compute_00.h>
+#include <decition/adc_tools/gps_distance.h>
+#include <decition/decition_type.h>
+#include <decition/adc_tools/transfer.h>
+#include <decition/obs_predict.h>
+#include <common/constants.h>
+#include <common/car_status.h>
+#include <math.h>
+#include <iostream>
+#include <fstream>
+#include <control/can.h>
+#include <time.h>
+#include <decition/adc_controller/base_controller.h>
+#include <decition/adc_controller/pid_controller.h>
+#include <decition/adc_planner/lane_change_planner.h>
+#include <decition/adc_planner/frenet_planner.h>
+#include <QTime>
+#include <iomanip>
+
+using namespace std;
+
+
+#include "ivlog.h"
+extern iv::Ivlog * givlog;
+
+#define PI (3.1415926535897932384626433832795)
+#define AVOID_NEW 1
+iv::decition::DecideGps00::DecideGps00()
+{
+    std::cout<<" init gps00"<<std::endl;
+}
+iv::decition::DecideGps00::~DecideGps00()
+{
+
+}
+
+float iv::decition::DecideGps00::xiuzhengCs=0;
+
+int iv::decition::DecideGps00::PathPoint = -1;
+double iv::decition::DecideGps00::minDis = iv::MaxValue;
+double iv::decition::DecideGps00::maxAngle = iv::MinValue;
+int iv::decition::DecideGps00::lastGpsIndex = 0;
+double iv::decition::DecideGps00::controlSpeed = 0;
+double iv::decition::DecideGps00::controlBrake = 0;
+double iv::decition::DecideGps00::controlAng = 0;
+double iv::decition::DecideGps00::Iv = 0;
+double iv::decition::DecideGps00::lastU = 0;
+double iv::decition::DecideGps00::lastVt = 0;
+double iv::decition::DecideGps00::lastEv = 0;
+
+int iv::decition::DecideGps00::gpsLineParkIndex = 0;
+int iv::decition::DecideGps00::gpsMapParkIndex = 0;
+double iv::decition::DecideGps00::lastDistance = MaxValue;
+double iv::decition::DecideGps00::lastPreObsDistance = MaxValue;
+double iv::decition::DecideGps00::obsDistance = -1;
+double iv::decition::DecideGps00::obsSpeed=0;
+double iv::decition::DecideGps00::obsDistanceAvoid = -1;
+double iv::decition::DecideGps00::lastDistanceAvoid = -1;
+
+double iv::decition::DecideGps00::lidarDistance = -1;
+double iv::decition::DecideGps00::myesrDistance = -1;
+double iv::decition::DecideGps00::lastLidarDis = -1;
+
+bool iv::decition::DecideGps00::parkMode = false;
+bool iv::decition::DecideGps00::readyParkMode = false;
+
+bool iv::decition::DecideGps00::zhucheMode = false;
+bool iv::decition::DecideGps00::readyZhucheMode = false;
+bool iv::decition::DecideGps00::hasZhuched = false;
+
+double iv::decition::DecideGps00::lastLidarDisAvoid = -1;
+double iv::decition::DecideGps00::lidarDistanceAvoid = -1;
+
+int iv::decition::DecideGps00::finishPointNum = -1;
+int iv::decition::DecideGps00::zhuchePointNum = 0;
+
+///kkcai, 2020-01-03
+bool iv::decition::DecideGps00::isFirstRun = true;
+//////////////////////////////////////////////
+
+float iv::decition::DecideGps00::minDecelerate=0;
+//bool iv::decition::DecideGps00::startingFlag = true;//起步标志，在起步时需要进行frenet规划。
+
+double offset_real = 0;
+double realspeed;
+double dSpeed;
+double dSecSpeed;
+double secSpeed;
+double ac;
+
+
+iv::Point2D obsPoint(-1, -1);
+iv::Point2D obsPointAvoid(-1, -1);
+
+
+int esrIndex = -1;
+int esrIndexAvoid = -1;
+double obsSpeedAvoid = 0;
+
+double esrDistance = -1;
+double esrDistanceAvoid = -1;
+int obsLostTime = 0;
+int obsLostTimeAvoid = 0;
+
+// double avoidTime = 0;
+
+double avoidXNewPre=0,avoidXNewPreFilter=0;//20230213,障碍物检测精度由1米换成0.5米
+//vector<int> avoidXNewPreVector;
+//int avoidXNew=0;
+//int avoidXNewLast=0;
+vector<double> avoidXNewPreVector;
+double avoidXNew=0;
+double avoidXNewLast=0;  //20230213,障碍物检测精度由1米换成0.5米
+double avoidX =0;
+int    turnLampFlag=0;  //  if <0:left , if >0:right
+float  roadWidth = 3.5;
+int roadSum =10;
+int roadNow = 0;
+int roadOri =0;
+int roadPre = -1;
+int lastRoadOri = 0;
+
+int lightTimes = 0;
+
+
+int lastRoadNum=1;
+
+int stopCount = 0;
+
+int gpsMissCount = 0;
+
+bool rapidStop = false;
+
+bool hasTrumpeted = false;
+
+
+bool changeRoad=false;
+//实验手刹
+bool handFirst = true;
+double handTimeSpan = 0;
+double handStartTime = 0;
+double handLastTime = 0;
+bool handPark = false;
+long handParkTime=10000;
+
+//喇叭
+bool trumpetFirstCount = true;
+double trumpetTimeSpan = 0;
+double trumpetStartTime = 0;
+double trumpetLastTime = 0;
+
+//过渡
+bool transferFirstCount = true;
+double transferTimeSpan = 0;
+double transferStartTime = 0;
+double transferLastTime = 0;
+
+bool busMode = false;
+bool parkBesideRoad = false;
+
+bool chaocheMode = false;
+bool specialSignle = false;
+
+double offsetX = 0;
+
+double steerSpeed=9000;
+
+bool transferPieriod;
+bool transferPieriod2;
+double traceDevi;
+
+bool startingFlag = true;  //起步标志，在起步时需要进行frenet规划。
+bool useFrenet = false;    //标志是否启用frenet算法避障
+bool useOldAvoid = true;   //标志是否用老方法避障
+bool front_car_decide_avoid=true;  //前后车距大于30米，后车允许根据自己障碍物情况避障，否则只能听从前车
+bool road_permit_avoid=false;  //true: 道路允许避障，false:道路不允许避障
+
+//数据存储功能 ,20210903,cxw
+bool file_cnt_add_en =false;  //用于提示是否需要将文件计数值增加
+bool file_cnt_add=false;
+bool map_start_point = true;
+bool first_start_en=true;  //自主巡迹数据存储用
+int  start_tracepoint;
+int  end_tracepoint;
+
+std::vector<std::vector<double>> doubledata;//大地坐标系下x,y,phi,delta，速度规划用变量
+
+enum VehState { normalRun, park, readyPark, stopObs, avoidObs, preAvoid, avoiding, backOri,preBack,
+                waitAvoid ,shoushaTest,justRun, changingRoad, chaoche, chaocheBack, reverseCar,reversing, reverseCircle, reverseDirect,reverseArr,
+                dRever,dRever0,dRever1,dRever2,dRever3,dRever4,dReverTZD,dReverTZDing} vehState,lastVehState;
+
+
+std::vector<iv::Point2D>  gpsTraceAim; //记录变道后的目标路径，若车辆经frenet规划行驶至该路径上后，就关闭frenet规划
+std::vector<iv::Point2D>  gpsTraceAvoid,gpsTraceNow,gpsTraceBack,gpsTracePre,gpsTraceOri,gpsTraceRear,gpsTraceNowLeft,gpsTraceNowRight,gpsTraceAvoidXY,gpsTraceMapOri;
+
+std::vector<double> esrDisVector(roadSum, -1);
+std::vector<double> lidarDisVector(roadSum, -1);
+std::vector<double> obsDisVector(roadSum,-1);
+std::vector<double> obsSpeedVector(roadSum, 0);
+
+std::vector<int> obsLostTimeVector(roadSum, 0);
+
+std::vector<double> lastLidarDisVector(roadSum, -1);
+std::vector<double> lastDistanceVector(roadSum, -1);
+
+std::vector<iv::Point2D> traceOriLeft,traceOriRight;
+
+bool qiedianCount = false;
+
+static int obstacle_avoid_flag=0;
+static int front_car_id=-1;
+static int front_car_vector_id=-1;
+static bool final_brake_lock=false,brake_mode=false;
+std::vector<iv::Point2D>  obsSpline;
+//日常展示
+
+#include <QDateTime>
+
+iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_gps_ins,
+                                                                   const std::vector<GPSData> gpsMapLine,
+                                                                   iv::LidarGridPtr lidarGridPtr,
+                                                                   std::vector<iv::Perception::PerceptionOutput> lidar_per,
+                                                                   iv::TrafficLight trafficLight)
+{
+    Decition gps_decition(new DecitionBasic);
+
+    bool bgroupgrpc = false;
+    qint64 ngrpcvalid = 3000;
+    iv::group::groupinfo xgroupgrpcinfo;
+    if((QDateTime::currentMSecsSinceEpoch() - ServiceCarStatus.mgroupgrpcupdatetime ) < ngrpcvalid)
+    {
+        ServiceCarStatus.mMutexgroupgrpc.lock();
+        xgroupgrpcinfo.CopyFrom(ServiceCarStatus.mgroupgrpcinfo);
+        ServiceCarStatus.mMutexgroupgrpc.unlock();
+        bgroupgrpc = true;
+    }
+    if((bgroupgrpc==true)&&(ServiceCarStatus.curID>1)){
+        for(int i=0;i<xgroupgrpcinfo.mvehinfo_size();i++){
+            if((xgroupgrpcinfo.mutable_mvehinfo(i)->intragroupid()+1)==ServiceCarStatus.curID){
+                front_car_id=xgroupgrpcinfo.mutable_mvehinfo(i)->intragroupid();
+                front_car_vector_id=i;
+            }
+        }
+    }else{
+        front_car_id=-1;
+    }
+
+    if(front_car_id>0){
+        if(xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->has_mgpsimu()){
+            front_car.front_car_ins.gps_lat=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().lat();
+            front_car.front_car_ins.gps_lng=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().lon();
+            front_car.front_car_ins.ins_heading_angle=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().heading();
+            front_car.front_car_ins.speed=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mgpsimu().speed();
+            GaussProjCal(front_car.front_car_ins.gps_lng,front_car.front_car_ins.gps_lat, &front_car.front_car_ins.gps_x, &front_car.front_car_ins.gps_y);
+            front_car.front_car_dis=GetDistance(now_gps_ins,front_car.front_car_ins);
+        }else{
+            front_car.front_car_dis=500;
+        }
+        if(xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->has_mcarstate()){
+            front_car.vehState=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mstate();
+            int avoidX_record=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mavoidx();
+            if(front_car.vehState!=0&&avoidX_record!=0)
+                front_car.avoidX=avoidX_record;
+            else
+                front_car.avoidX=0;
+            front_car.obs_distance=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mobsdistance();
+            front_car.obs_speed=xgroupgrpcinfo.mutable_mvehinfo(front_car_vector_id)->mcarstate().mobsspeed();
+        }
+        if(ServiceCarStatus.txt_log_on==true)
+        {
+            givlog->debug("decition_brain","front_car_id: %d,FrontState: %d,FrontAvoidX: %d,FrontDis: %f,FrontObs: %f",
+                      front_car_id,front_car.vehState,front_car.avoidX,front_car.front_car_dis,front_car.obs_distance);
+        }
+    }
+
+    if(front_car_id>0){
+        if(front_car.front_car_dis>35){
+            front_car_decide_avoid=true;
+        }else if((front_car.front_car_dis<=35)&&(front_car.avoidX==0)){
+            front_car_decide_avoid=false;
+        }else if((front_car.front_car_dis<=35)&&(front_car.avoidX!=0)){
+            front_car_decide_avoid=true;
+        }
+    }else{
+        front_car_decide_avoid=true;
+    }
+
+
+    static int file_num;//log文件存储计数
+    if (isFirstRun)
+    {
+        file_num=0;
+        initMethods();
+        vehState = normalRun;
+        startAvoid_gps_ins = now_gps_ins;
+        init();
+        PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
+                                                          DecideGps00::lastGpsIndex,
+                                                          DecideGps00::minDis,
+                                                          DecideGps00::maxAngle);
+        DecideGps00::lastGpsIndex = PathPoint;
+
+
+        if(ServiceCarStatus.speed_control==true){
+            Compute00().getPlanSpeed(gpsMapLine);
+        }
+
+        double dis =  GetDistance(*gpsMapLine[0], *gpsMapLine[gpsMapLine.size()-1]);
+        if(dis<1){
+            circleMode=true;
+        }else{
+            circleMode=false;
+        }
+
+        getV2XTrafficPositionVector(gpsMapLine);
+
+        ServiceCarStatus.bocheMode=false;
+        ServiceCarStatus.daocheMode=false;
+        parkMode=false;
+        readyParkMode=false;
+        finishPointNum=-1;
+        roadNowStart=true;
+        isFirstRun = false;
+        obstacle_avoid_flag=0;
+        avoidXNew=0;
+        avoidXNewLast=0;
+        avoidXNewPre=0;
+        avoidXNewPreFilter=0;
+        gpsMapLine[gpsMapLine.size()-1]->frenet_s=0;
+
+        if((ServiceCarStatus.txt_log_on==true)&&(ServiceCarStatus.msysparam.mvehtype=="hapo"))
+        {
+
+            givlog->debug("decition_brain_bool","DoorTimeStart: %d,current: %d",0,0);
+        }
+    }
+
+    ServiceCarStatus.mvehState=vehState;
+    ServiceCarStatus.mavoidX=avoidXNew;
+
+    changingDangWei=false;
+    minDecelerate=0;
+
+    busMode = ServiceCarStatus.busmode;
+    nearStation=false;
+
+    gps_decition->leftlamp = false;
+    gps_decition->rightlamp = false;
+
+    xiuzhengCs=0;
+    is_arrivaled = false;
+
+    realspeed = now_gps_ins.speed;//km/h
+    secSpeed = realspeed / 3.6;//m/s
+
+
+    if(front_car_decide_avoid == false)    //geely logic yizhi
+    {
+        obstacle_avoid_flag =  0;
+    }
+
+
+    if(ServiceCarStatus.msysparam.gpsOffset_y!=0 || ServiceCarStatus.msysparam.gpsOffset_x!=0){
+        GPS_INS gpsOffset = Coordinate_UnTransfer(ServiceCarStatus.msysparam.gpsOffset_x, ServiceCarStatus.msysparam.gpsOffset_y, now_gps_ins);
+        now_gps_ins.gps_x=gpsOffset.gps_x;
+        now_gps_ins.gps_y=gpsOffset.gps_y;
+        GaussProjInvCal(now_gps_ins.gps_x,  now_gps_ins.gps_y, &now_gps_ins.gps_lng, &now_gps_ins.gps_lat);
+    }
+
+
+    if(now_gps_ins.gps_lat<0||now_gps_ins.gps_lat>90)
+    {
+        gps_decition->wheel_angle = 0.2;
+        gps_decition->speed = dSpeed;
+
+        gps_decition->accelerator = -0.5;
+        gps_decition->torque = 0.0;
+//        gps_decition->brake=10;
+        if(ServiceCarStatus.msysparam.mvehtype=="shenlan")  //长安深蓝决策发的brake是负值的时候才刹车，利用的是扭矩
+        {
+          gps_decition->brake=-10;
+        }
+        else
+        {
+           gps_decition->brake=10;
+        }
+        givlog->debug("decition_brain","gps_lat is error,f%",now_gps_ins.gps_lat); //20230315
+        return gps_decition;
+    }
+
+    if(ServiceCarStatus.daocheMode){
+
+        now_gps_ins.ins_heading_angle=now_gps_ins.ins_heading_angle+180;
+        if( now_gps_ins.ins_heading_angle>=360)
+            now_gps_ins.ins_heading_angle= now_gps_ins.ins_heading_angle-360;
+    }
+    //    qDebug("heading is %g",now_gps_ins.ins_heading_angle);
+
+/****************************self park logic begin************************************************/
+    aim_gps_ins.gps_lat= ServiceCarStatus.mfParkLat;
+    aim_gps_ins.gps_lng= ServiceCarStatus.mfParkLng;
+    aim_gps_ins.ins_heading_angle= ServiceCarStatus.mfParkHeading;
+    GaussProjCal(aim_gps_ins.gps_lng,aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+
+    //sidePark
+    if(ServiceCarStatus.mnParkType==1)
+    {
+        if( vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 &&  vehState!=dRever2
+                &&  vehState!=dRever3 && vehState!=dRever4 && vehState!=reverseArr )
+        {
+            int bocheEN=Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
+            if(bocheEN==1){
+                ServiceCarStatus.bocheEnable=1;
+            }else if(bocheEN==0){
+                ServiceCarStatus.bocheEnable=0;
+            }
+        }
+        else
+        {
+            ServiceCarStatus.bocheEnable=2;
+        }
+
+
+
+        if(ServiceCarStatus.bocheMode && vehState!=dRever &&  vehState!=dRever0  &&  vehState!=dRever1 && vehState!=dRever2
+                &&  vehState!=dRever3 && vehState!=dRever4 &&  vehState!=reverseArr )
+        {
+            if(abs(realspeed)>0.1)
+            {
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                gps_decition->wheel_angle=0;
+                return gps_decition;
+            }else
+            {
+                if(trumpet()>3000)
+                {
+                    trumpetFirstCount=true;
+                    vehState=dRever;
+                }
+            }
+        }
+
+        if (vehState == dRever)
+        {
+            GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+            Compute00().bocheDirectCompute(now_gps_ins, aim_gps_ins);
+            vehState = dRever0;
+            qiedianCount=true;
+            std::cout<<"enter side boche mode"<<std::endl;
+        }
+        if (vehState == dRever0)
+        {
+            double dis = GetDistance(now_gps_ins, Compute00().dTpoint0);
+            if (dis<2.0)
+            {
+                vehState = dRever1;
+                qiedianCount = true;
+                cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
+            }
+            else
+            {
+                controlAng = 0;
+                dSpeed = 2;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->wheel_angle = 0;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                //	gps_decition->accelerator = 0;
+                return gps_decition;
+            }
+        }
+
+        if (vehState == dRever1)
+        {
+            double dis = GetDistance(now_gps_ins, Compute00().dTpoint1);
+            if(dis<2.0)
+            {
+                vehState = dRever2;
+                qiedianCount = true;
+                cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+            }
+            else
+            {
+                controlAng = Compute00().dBocheAngle*16.5;
+                gps_decition->wheel_angle = 0 - controlAng;
+                if (qiedianCount && trumpet()<1000)
+                {
+                    //                gps_decition->brake = 10;
+                    //                gps_decition->torque = 0;
+                    dSpeed=0;
+                    minDecelerate=min(minDecelerate,-0.5f);
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                else
+                {
+                    qiedianCount = false;
+                    trumpetFirstCount = true;
+                    dSpeed = 2;
+                    dSecSpeed = dSpeed / 3.6;
+                    gps_decition->speed = dSpeed;
+                    obsDistance=-1;
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+
+
+
+                cout<<"farTpointDistance================"<<dis<<endl;
+                return gps_decition;
+            }
+        }
+
+
+        if (vehState == dRever2)
+        {
+            double dis = GetDistance(now_gps_ins, Compute00().dTpoint2);
+            Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+            Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint2.gps_x,Compute00().dTpoint2.gps_y, aim_gps_ins);
+            double xx= (pt1.x-pt2.x);
+            // if(xx>0)
+            if(xx>-0.5)
+            {
+                GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+                Point2D ptt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+                double xxx=ptt.x;
+                double yyy =ptt.y;
+                vehState = dRever3;
+                qiedianCount = true;
+                cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+                cout<<"xxx+++++++++++++++++++++++++++++++++"<<xxx<<endl;
+                cout<<"yyy+++++++++++++++++++++++++++++++++"<<yyy<<endl;
+            }
+            else
+            {
+                if (qiedianCount && trumpet()<1000)
+                {
+                    /*  gps_decition->brake = 10;
+                    gps_decition->torque = 0;  */
+                    dSpeed=0;
+                    minDecelerate=min(minDecelerate,-0.5f);
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                else
+                {
+                    qiedianCount = false;
+                    trumpetFirstCount = true;
+
+                    dSpeed = 2;
+                    dSecSpeed = dSpeed / 3.6;
+                    gps_decition->speed = dSpeed;
+                    obsDistance=-1;
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+
+                gps_decition->wheel_angle = 0 ;
+                cout<<"farTpointDistance================"<<dis<<endl;
+                return gps_decition;
+            }
+        }
+
+
+        if (vehState == dRever3)
+        {
+            double dis = GetDistance(now_gps_ins, Compute00().dTpoint3);
+            double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
+            if((((angdis<5)||(angdis>355)))&&(dis<10.0))
+            {
+                vehState = dRever4;
+                qiedianCount = true;
+                cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+            }
+            else {
+
+                controlAng = 0-Compute00().dBocheAngle*16.5;
+                gps_decition->wheel_angle = 0 - controlAng*0.95;
+                if (qiedianCount && trumpet()<1000)
+                {
+                    //                gps_decition->brake = 10;
+                    //                gps_decition->torque = 0;
+                    dSpeed=0;
+                    minDecelerate=min(minDecelerate,-0.5f);
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                else
+                {
+                    qiedianCount = false;
+                    trumpetFirstCount = true;
+                    dSpeed = 2;
+                    dSecSpeed = dSpeed / 3.6;
+                    gps_decition->speed = dSpeed;
+                    obsDistance=-1;
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+
+
+                cout<<"farTpointDistance================"<<dis<<endl;
+                return gps_decition;
+            }
+        }
+
+        if (vehState == dRever4)
+        {
+            //	double dis = GetDistance(now_gps_ins, aim_gps_ins);
+            Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+
+            if ((pt.y)<0.5)
+            {
+                qiedianCount=true;
+                vehState=reverseArr;
+                cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
+                //            gps_decition->accelerator = -3;
+                //            gps_decition->brake =10 ;
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+
+                gps_decition->wheel_angle=0;
+                return gps_decition;
+            }
+            else
+            {
+                //if (qiedianCount && trumpet()<0)
+                if (qiedianCount && trumpet()<1000)
+                {
+                    dSpeed=0;
+                    minDecelerate=min(minDecelerate,-0.5f);
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                else
+                {
+                    qiedianCount = false;
+                    trumpetFirstCount = true;
+                    dSpeed = 2;
+                    dSecSpeed = dSpeed / 3.6;
+                    gps_decition->speed = dSpeed;
+                    obsDistance=-1;
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                controlAng = 0;
+                gps_decition->wheel_angle = 0;
+                return gps_decition;
+            }
+        }
+    }
+
+
+    //chuizhiPark
+    if(ServiceCarStatus.mnParkType==0)
+    {
+        if( vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect &&  vehState!=reverseArr )
+        {
+            int bocheEN=Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
+            if(bocheEN==1)
+            {
+                ServiceCarStatus.bocheEnable=1;
+            }
+            else if(bocheEN==0)
+            {
+                ServiceCarStatus.bocheEnable=0;
+            }
+        }
+        else
+        {
+            ServiceCarStatus.bocheEnable=2;
+        }
+
+        if(ServiceCarStatus.bocheMode && vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle
+                &&  vehState!=reverseDirect&&  vehState!=reverseArr )
+        {
+            if(abs(realspeed)>0.1)
+            {
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                gps_decition->wheel_angle=0;
+                return gps_decition;
+            }
+            else
+            {
+                if(trumpet()>3000)
+                {
+                    trumpetFirstCount=true;
+                    vehState=reverseCar;
+                }
+            }
+        }
+
+        if (vehState == reverseCar)
+        {
+            Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
+            vehState = reversing;
+            qiedianCount=true;
+        }
+        if (vehState == reversing)
+        {
+            double dis = GetDistance(now_gps_ins, Compute00().nearTpoint);
+            iv::Point2D pt = iv::decition::Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y,aim_gps_ins);
+            iv::Point2D ptnear = iv::decition::Coordinate_Transfer(Compute00().nearTpoint.gps_x,Compute00().nearTpoint.gps_y,aim_gps_ins);
+
+             double fdistonear = fabs(pt.x - ptnear.x);
+             if(fdistonear<0.5)  //将吉利项目中的停车逻辑移植过来
+    //        if (dis<2.0)//0.5
+            {
+                vehState = reverseCircle;
+                qiedianCount = true;
+                cout<<"到达近切点+++++++++++++++++++++++++++++++++"<<endl;
+            }
+            else
+            {
+                controlAng = 0;
+                dSpeed = 2;
+                dSecSpeed = dSpeed / 3.6;
+                gps_decition->wheel_angle = 0;
+                gps_decition->speed = dSpeed;
+                obsDistance=-1;
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                //	gps_decition->accelerator = 0;
+                return gps_decition;
+            }
+        }
+
+        if (vehState == reverseCircle)
+        {
+            double dis = GetDistance(now_gps_ins, Compute00().farTpoint);
+            double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
+            if((((angdis<5)||(angdis>355)))&&(dis<3.0))
+                //       if((((angdis<4)||(angdis>356)))&&(dis<2.0))
+            {
+                vehState = reverseDirect;
+                qiedianCount = true;
+                cout<<"到达远切点+++++++++++++++++++++++++++++++++"<<endl;
+
+            }
+            else {
+                if (qiedianCount && trumpet()<0)
+                    //         if (qiedianCount && trumpet()<1500)
+                {
+
+                    //                gps_decition->brake = 10;
+                    //                gps_decition->torque = 0;
+                    dSpeed=0;
+                    minDecelerate=min(minDecelerate,-0.5f);
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                else
+                {
+                    qiedianCount = false;
+                    trumpetFirstCount = true;
+
+
+                    dSpeed = 2;
+                    dSecSpeed = dSpeed / 3.6;
+                    gps_decition->speed = dSpeed;
+                    obsDistance=-1;
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+
+                }
+
+                controlAng = Compute00().bocheAngle*16.5;
+                gps_decition->wheel_angle = 0 - controlAng*1.05;
+
+                cout<<"farTpointDistance================"<<dis<<endl;
+
+                return gps_decition;
+            }
+        }
+
+        if (vehState == reverseDirect)
+        {
+            //	double dis = GetDistance(now_gps_ins, aim_gps_ins);
+            Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+
+            if ((pt.y)<0.5)
+            {
+
+                qiedianCount=true;
+                vehState=reverseArr;
+                cout<<"到达泊车点+++++++++++++++++++++++++++++++++"<<endl;
+                //            gps_decition->accelerator = -3;
+                //            gps_decition->brake = 10;
+                //            gps_decition->torque = 0;
+                gps_decition->wheel_angle=0;
+                dSpeed=0;
+                minDecelerate=min(minDecelerate,-0.5f);
+                phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                return gps_decition;
+            }
+            else {
+
+                //if (qiedianCount && trumpet()<0)
+                if (qiedianCount && trumpet()<1000)
+                {
+
+                    //                gps_decition->brake = 10;
+                    //                gps_decition->torque = 0;
+                    dSpeed=0;
+                    minDecelerate=min(minDecelerate,-0.5f);
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                else
+                {
+                    qiedianCount = false;
+                    trumpetFirstCount = true;
+                    dSpeed = 1;
+                    dSecSpeed = dSpeed / 3.6;
+                    gps_decition->speed = dSpeed;
+                    obsDistance=-1;
+                    phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                }
+                controlAng = 0;
+                gps_decition->wheel_angle = 0;
+                return gps_decition;
+            }
+        }
+    }
+
+    if(ServiceCarStatus.mnParkType==2)//guosai pingtai yanzheng 直接倒车
+    {
+        ServiceCarStatus.bocheEnable=1;
+        if(ServiceCarStatus.bocheMode &&(vehState != dReverTZD))
+        {
+            vehState = dReverTZD;
+        }
+
+        if(vehState == dReverTZD)
+        {
+             Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
+             if(pt.y<0.5)
+             {
+                 if(abs(realspeed)>0.1)
+                 {
+                     vehState = reverseArr;
+                 }
+                 gps_decition->wheel_angle = 0;
+                 gps_decition->speed = dSpeed;
+                 obsDistance=-1;
+                 minDecelerate=min(minDecelerate,-0.5f);
+                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                 //	gps_decition->accelerator = 0;
+                 return gps_decition;
+             }
+             else
+             {
+                 gps_decition->wheel_angle = 0;
+                 gps_decition->speed = dSpeed;
+                 obsDistance=-1;
+                 dSpeed = 2;
+                 dSecSpeed = dSpeed / 3.6;
+                 gps_decition->speed = dSpeed;
+                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+                 //	gps_decition->accelerator = 0;
+                 return gps_decition;
+             }
+        }
+    }
+
+    if ((vehState == reverseArr)&&((ServiceCarStatus.mnParkType==1)||(ServiceCarStatus.mnParkType==0)
+                                   ||(ServiceCarStatus.mnParkType==2))) //不论哪种泊车最后都会到这个状态
+    {
+        ServiceCarStatus.bocheMode=false;
+        if (qiedianCount && trumpet()<1500)
+        {
+            dSpeed=0;
+            minDecelerate=min(minDecelerate,-0.5f);
+            phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
+            ServiceCarStatus.bocheMode=false;
+        }
+        else
+        {
+            qiedianCount = false;
+            trumpetFirstCount = true;
+            ServiceCarStatus.bocheEnable=0;
+            vehState=normalRun;
+            ServiceCarStatus.mbRunPause=true;
+            ServiceCarStatus.mnBocheComplete=100;
+            cout<<"泊车daowei mbRunPause+++++++++++++++++++++++++++++++++"<<endl;
+        }
+        gps_decition->wheel_angle = 0 ;
+        return gps_decition;
+    }
+
+/****************************self park logic end************************************************/
+
+    obsDistance = -1;
+    esrIndex = -1;
+    lidarDistance = -1;
+
+    obsDistanceAvoid = -1;
+    esrIndexAvoid = -1;
+    roadPre = -1;
+    avoidXNewPre=0;
+    //dSpeed = ServiceCarStatus.mroadmode_vel.mfmode0;
+    int nmapsize = gpsMapLine.size();
+
+    gpsTraceOri.clear();
+    gpsTraceNow.clear();
+    gpsTraceAim.clear();
+    gpsTraceAvoid.clear();
+    gpsTraceBack.clear();
+    gpsTraceNowLeft.clear();
+    gpsTraceNowRight.clear();
+
+    dSpeed =80;
+
+    if (!isFirstRun)
+    {
+        PathPoint = Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
+        if(PathPoint<0)
+        {
+            PathPoint = Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine, DecideGps00::lastGpsIndex, DecideGps00::minDis, DecideGps00::maxAngle);
+        }
+    }
+
+
+    if (PathPoint<0)
+    {
+        minDecelerate=-1.0;
+        phaseSpeedDecition(gps_decition, secSpeed, -1, 0,now_gps_ins);
+
+        givlog->debug("decition_brain","PathPoint is smaller than 0, it is : %d",PathPoint);
+        return gps_decition;
+    }
+
+    DecideGps00::lastGpsIndex = PathPoint;
+
+    if(!ServiceCarStatus.inRoadAvoid)
+    {
+        roadOri = gpsMapLine[PathPoint]->roadOri;
+        roadSum = gpsMapLine[PathPoint]->roadSum;
+
+    }
+    else
+    {
+        roadOri=gpsMapLine[PathPoint]->roadOri*3+1;
+        roadSum = gpsMapLine[PathPoint]->roadSum*3;
+    }
+
+    if(roadNowStart)
+    {
+        roadNow=roadOri;
+        roadNowStart=false;
+    }
+
+
+    if(ServiceCarStatus.avoidObs)
+    {
+        gpsMapLine[PathPoint]->runMode=1;
+    }
+    else
+    {
+        gpsMapLine[PathPoint]->runMode=0;
+    }
+
+#ifdef AVOID_NEW
+    if(obstacle_avoid_flag==1)
+    {
+
+    }
+    else
+    {
+        avoidXNew=0;
+        roadNow = roadOri;
+        if(vehState==backOri)
+        {
+            vehState=normalRun;
+        }
+    }
+    givlog->debug("decition_brain","avoidXNew: %f",avoidXNew);
+#else
+    if(obstacle_avoid_flag==1)
+    {
+        avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint]);
+    }
+    else
+    {
+        avoidX=0;
+        roadNow = roadOri;
+        if(vehState==backOri)
+        {
+            vehState=normalRun;
+        }
+    }
+#endif
+
+    if ( gpsMapLine[PathPoint]->runMode == 0 && (vehState == avoidObs || vehState == stopObs || vehState == preAvoid
+                                                 ||  vehState == avoiding || vehState == backOri || vehState ==preBack || vehState ==waitAvoid ) )
+    {
+        vehState = normalRun;
+        roadNow = roadOri;
+    }
+
+    gpsTraceOri= getGpsTrace(now_gps_ins, gpsMapLine, lastGpsIndex,circleMode);
+    gpsTraceMapOri=gpsTraceOri;
+
+    if((vehState == avoiding)&&(obstacle_avoid_flag == 1))
+    {
+        iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);
+        if((now_s_d.s+20>(gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s))&&((gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s)<((*gpsMapLine[gpsMapLine.size()-1]).frenet_s)))
+        {
+            double sPathFinal=gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s;
+            if(sPathFinal+20<=(*gpsMapLine[gpsMapLine.size()-1]).frenet_s)
+            {
+                sPathFinal=sPathFinal+20;
+            }
+            else
+            {
+                sPathFinal=(*gpsMapLine[gpsMapLine.size()-1]).frenet_s;
+            }
+            for(double s=gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s;s<=sPathFinal;s+=0.1)
+            {
+                iv::Point2D gpsTracePoint=frenet_to_cartesian1D(gpsMapLine,s,-avoidXNew,PathPoint);
+                gpsTraceAvoidXY.push_back(gpsTracePoint);
+            }
+        }
+    }
+
+    if((vehState == avoiding || vehState == backOri) && (gpsTraceAvoidXY.size()>0) && (obstacle_avoid_flag==1))
+    {
+        gpsTraceOri=getGpsTraceAvoid(now_gps_ins, gpsTraceAvoidXY, lastGpsIndex,circleMode);
+    }
+
+    if(gpsTraceOri.empty())
+    {
+        gps_decition->wheel_angle = 0.2;
+        gps_decition->speed = dSpeed;
+
+        gps_decition->accelerator = -0.5;
+        gps_decition->torque = 0.0;
+        //gps_decition->brake=10; //
+        if(ServiceCarStatus.msysparam.mvehtype=="shenlan")  //长安深蓝决策发的brake是负值的时候才刹车，利用的是扭矩
+        {
+          gps_decition->brake=-20;
+        }
+        else
+        {
+           gps_decition->brake=10;
+        }
+        givlog->warn("decition_brain","gpsTraceOri is empty");
+        return gps_decition;
+    }
+
+
+
+//    traceDevi=gpsTraceOri[0].x;
+
+    //起步进行frenet规划后，车辆行驶至地图路线上，起步操作完毕。
+    //如果车辆本身启动位置在地图路线上，可不用进行frenet规划。
+    //    if(GetDistance(now_gps_ins,*gpsMapLine[PathPoint]) < 1){
+    //        startingFlag = false;
+    //    }
+//    startingFlag = false;
+//    if(startingFlag)
+//    {
+//        //        gpsTraceAim = gpsTraceOri;
+//        if(abs(gpsTraceOri[0].x)>1)
+//        {
+//            cout<<"frenetPlanner->getPath:pre"<<endl;
+//            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,0,lidarGridPtr);
+//            cout<<"frenetPlanner->getPath,gpsTraceNow.size: "<<gpsTraceNow.size()<<endl;
+//            if(gpsTraceNow.size()==0)
+//            {
+//                gps_decition->accelerator = 0;
+//                gps_decition->brake=10;
+//                gps_decition->wheel_angle = lastAngle;
+//                gps_decition->speed = 0;
+//                return gps_decition;
+//            }
+//        }
+//        else
+//        {
+//            startingFlag = false;
+//        }
+//    }
+
+
+//    if(useFrenet){
+//        //如果车辆在变道中，启用frenet规划局部路径
+//        if(vehState == avoiding || vehState == backOri)
+//        {
+//            // avoidX = (roadOri - roadNow)*roadWidth;
+//            avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint]);
+//            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+//            //gpsTraceAim = laneChangePlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+//            gpsTraceAim = getGpsTraceOffset(gpsTraceOri, avoidX);
+//            if(gpsTraceNow.size()==0)
+//            {
+//                gps_decition->accelerator = 0;
+//                gps_decition->brake=10;
+//                gps_decition->wheel_angle = lastAngle;
+//                gps_decition->speed = 0;
+//                return gps_decition;
+//            }
+//        }
+//    }
+
+#ifdef AVOID_NEW
+    if(gpsTraceNow.size()==0)
+    {
+        if (avoidXNew==0.0)
+        {
+            if((vehState == backOri)||(vehState == avoiding))
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+            else
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
+                gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
+            }
+        }
+        else
+        {
+            if((vehState == avoiding)||(vehState == backOri))
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+            else
+            {
+                gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidXNew);//luojixuyao xiugai
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+        }
+    }
+#else
+    if(gpsTraceNow.size()==0){
+        if (roadNow==roadOri)
+        {
+            if(vehState == backOri)
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+            else{
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                gpsTraceNowLeft.assign(traceOriLeft.begin(),traceOriLeft.end());
+                gpsTraceNowRight.assign(traceOriRight.begin(),traceOriRight.end());
+            }
+        }else
+        {
+            if((vehState == avoiding)||(vehState == backOri))
+            {
+                gpsTraceNow.assign(gpsTraceOri.begin(), gpsTraceOri.end());
+                //                gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
+                //                gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }else{
+                gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
+                //                gpsTraceNowLeft=getGpsTraceOffset(traceOriLeft, avoidX);
+                //                gpsTraceNowRight=getGpsTraceOffset(traceOriRight, avoidX);
+                getGpsTraceNowLeftRight(gpsTraceNow);
+            }
+        }
+    }
+
+#endif
+
+
+
+    if(vehState==normalRun)
+    {
+        if(gpsTraceNow.size()>200)
+        {
+            if(gpsTraceNow[200].x<-3)
+            {
+                gps_decition->leftlamp = true;
+                gps_decition->rightlamp = false;
+            }
+            else if(gpsTraceNow[200].x>3)
+            {
+                gps_decition->leftlamp = false;
+                gps_decition->rightlamp = true;
+            }
+            else
+            {
+                gps_decition->leftlamp = false;
+                gps_decition->rightlamp = false;
+            }
+        }
+    }
+
+    //  dSpeed = getSpeed(gpsTraceNow);
+
+
+    planTrace.clear();//Add By YuChuli, 2020.11.26
+    for(int i=0;i<gpsTraceNow.size();i++){
+        TracePoint pt(gpsTraceNow[i].x,gpsTraceNow[i].y);
+        planTrace.push_back(pt);
+    }
+
+    controlAng= getAngle(gpsTraceNow,now_gps_ins,gps_decition);
+
+    /****************************terminal point stop logic begin************************************************/
+    //转角
+    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+    {
+        if(!circleMode && PathPoint>(gpsMapLine.size()-20))
+        {
+            controlAng=0;
+        }
+    }
+    else
+    {
+        if(!circleMode && PathPoint>(gpsMapLine.size()-250))
+        {
+            if(realspeed<0.5)
+                controlAng=0;
+        }
+    }
+    //加速度
+    double acc_end = 0.1;
+    static double distoend=1000;
+
+    int useaccend = 1;
+    double mstopbrake=0.6;
+
+    if(((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))||(useaccend == 1))
+    {
+                if(PathPoint+1000>gpsMapLine.size()){
+                    distoend=computeDistToEnd(gpsMapLine,PathPoint);
+                }else{
+                    distoend=1000;
+                }
+                if(!circleMode && distoend<100){
+                        if(distoend<3.0)
+                        {
+                            std::cout<<"distoend:  "<<distoend<<std::endl;
+                        }
+                        double nowspeed = realspeed/3.6;
+                        double brakedis = 100;
+                        double stopbrake = mstopbrake;
+                        if(nowspeed>10)
+                        {
+                            brakedis = (nowspeed*nowspeed)/(2*2.0);
+                        }
+                        else
+                        {
+                            brakedis = (nowspeed*nowspeed)/(2*stopbrake)+3;
+                        }
+                        if((distoend<3)||(distoend<brakedis))
+                        {
+                            if(distoend == 0.0)distoend = 0.09;
+                            acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
+                            if((acc_end<(-3.0))&&(nowspeed<10))acc_end = -3.0;
+                        }
+
+                        if((distoend <= 0.1)||(PathPoint>=gpsMapLine.size()-2))acc_end = (stopbrake + 0.1)*(-1.0);
+
+
+                        if(acc_end<0)minDecelerate = acc_end;
+
+                        givlog->debug("decition_brain","acc_end: %f",acc_end);
+
+                }
+    }
+    else
+    {
+        //                if(!circleMode && PathPoint>gpsMapLine.size()-brake_distance){
+        //                        minDecelerate=-0.5;
+        //                        std::cout<<"map finish brake"<<std::endl;
+        //                }
+        if(!circleMode){
+            double forecast_final=secSpeed*secSpeed+5;
+            int forecast_final_point=((int)forecast_final)*10+1500;
+            static int BrakePoint=-1;
+            static bool final_brake=false;
+            static double distance_to_end=1000;
+            static bool enterTofinal=false;  //20230417,shenlan
+            static double enterTofinal_speed=200;
+            if(PathPoint+forecast_final_point>gpsMapLine.size())
+            {
+                distance_to_end=computeDistToEnd(gpsMapLine,PathPoint);
+                if(ServiceCarStatus.txt_log_on==true)
+                {
+                    givlog->debug("decition_brain","distoend: %f",distance_to_end);
+                }
+                if((forecast_final>=distance_to_end)&&(realspeed>3))
+                {
+                    final_brake=true;
+                    if(BrakePoint==-1)
+                        BrakePoint=PathPoint-10;
+                }
+            }
+            else
+            {
+                distance_to_end=1000;
+            }
+            if(PathPoint<BrakePoint)
+            {
+                final_brake=false;
+                final_brake_lock=false;
+                brake_mode=false;
+                BrakePoint=-1;
+                enterTofinal=false;  //20230417,shenlan
+                enterTofinal_speed=200;
+            }
+            if(final_brake==true)
+            {
+                if((realspeed>3)&&(final_brake_lock==false))
+                {
+                    minDecelerate=-0.7;
+                }
+                else
+                {
+                    if(enterTofinal==false)  //20230417,shenlan)
+                    {
+                        enterTofinal_speed=realspeed;
+                        enterTofinal=true;
+                    }
+                    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+                    {
+                       dSpeed=min(enterTofinal_speed, 3.0);
+                    }
+                    else
+                    {
+                       dSpeed=min(dSpeed, 3.0);
+                    }
+                    final_brake_lock=true;
+                    brake_mode=true;
+                    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+                    {
+                        if(distance_to_end<2.0)
+                        {
+                            minDecelerate=-0.7;
+                        }
+
+                    }
+                    else
+                    {
+                        if(distance_to_end<0.8)
+                        {
+                            minDecelerate=-0.7;
+                        }
+                    }
+                }
+            }
+        }
+    }
+    if(brake_mode==true){
+        dSpeed=min(dSpeed, 3.0);
+    }
+    /****************************terminal point stop logic end************************************************/
+    //group map end park pro--------start
+    if(front_car_id>0){
+        static  bool final_lock=false;
+        if((distoend<90)&&(front_car.front_car_dis<obsDistance+10)){
+            if((obsDistance>3)&&(obsDistance<20)){
+                if((realspeed>3)&&(final_lock==false)){
+                    minDecelerate=-0.7;
+                }else{
+                    dSpeed = min(3.0,dSpeed);
+                    final_lock=true;
+                }
+                obsDistance=200;
+            }else if((obsDistance>1)&&(obsDistance<3)){
+                minDecelerate=-0.5;
+                obsDistance=200;
+            }else if(obsDistance<1){
+                minDecelerate=-1.0;
+            }
+            if(realspeed<1){
+                final_lock=false;
+            }
+        }
+    }
+    //group map end park pro-----------end
+
+    if(front_car_id>0)
+    {
+        static bool brake_state=false;
+        if(((front_car.vehState!=0)||(front_car.avoidX!=0))&&(front_car.front_car_dis<15))
+        {
+            brake_state=true;
+        }
+        if(brake_state==true)
+        {
+            dSpeed=min(dSpeed,ServiceCarStatus.mroadmode_vel.mfmode18);
+        }
+        if(((front_car.vehState==0)&&(front_car.avoidX==0))||(front_car.front_car_dis>25))
+        {
+            brake_state=false;
+        }
+
+    }
+
+
+
+    //2020-0106
+//    if(ServiceCarStatus.msysparam.mvehtype !="zhongche")
+//    {
+
+//        if(vehState==avoiding){
+//            ServiceCarStatus.msysparam.vehWidth=2.4;
+//            controlAng=max(-150.0,controlAng);//35  zj-150
+//            controlAng=min(150.0,controlAng);//35   zj-150
+//        }
+//        if(vehState==backOri){
+//            controlAng=max(-150.0,controlAng);//35   zj-150
+//            controlAng=min(150.0,controlAng);//35     zj-150
+//        }
+//    }
+
+//    givlog->debug("brain_decition","vehState: %d,controlAng: %f",vehState,controlAng);
+
+    if(ServiceCarStatus.speed_control==true)
+    {
+        dSpeed = min(doubledata[PathPoint][4],dSpeed);
+    }
+    else
+    {
+        if (gpsMapLine[PathPoint]->roadMode ==0)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = false;
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 5)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode5,dSpeed);
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = false;
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 11)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode11,dSpeed);
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 14)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode14,dSpeed);
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 15)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode15,dSpeed);
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 16)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode16,dSpeed);
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 17)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode17,dSpeed);
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 18)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode18,dSpeed);
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 1)
+        {
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode1,dSpeed);
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 2)
+        {
+            dSpeed = min(15.0,dSpeed);
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 7)
+        {
+            dSpeed = min(15.0,dSpeed);
+            xiuzhengCs=1.5;
+        }
+        else if (gpsMapLine[PathPoint]->roadMode == 4) //1220
+        {
+            dSpeed = min(4.0,dSpeed);
+        }
+        else{
+            dSpeed = min(ServiceCarStatus.mroadmode_vel.mfmode0,dSpeed);
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = false;
+        }
+    }
+
+    if (gpsMapLine[PathPoint]->speed_mode == 2)
+    {
+        dSpeed = min(25.0,dSpeed);
+
+    }
+
+    if((gpsMapLine[PathPoint]->speed)>0.001)
+    {
+        dSpeed = min((gpsMapLine[PathPoint]->speed*3.6),dSpeed);
+        //        if((gpsMapLine[PathPoint]->speed)>4.5)
+        //        {
+        //            dSpeed =gpsMapLine[PathPoint]->speed*3.6;
+        //        }
+    }
+
+    //givlog->debug("decition_brain","brake_mode: %d",brake_mode);
+
+#ifdef AVOID_NEW  //20220223toggle
+    //    if((vehState==avoiding)||(vehState==preAvoid))
+    //    {
+    //        dSpeed = min(8.0,dSpeed);
+    //    }else if((vehState==backOri)||(avoidXNew!=0)){
+    //        dSpeed = min(12.0,dSpeed);
+    //    }
+#else
+    if((vehState==avoiding)||(vehState==backOri)||(roadNow != roadOri)||(vehState==preAvoid))
+    {
+        dSpeed = min(8.0,dSpeed);
+    }
+#endif
+
+
+
+
+    dSpeed = limitSpeed(controlAng, dSpeed);
+    dSecSpeed = dSpeed / 3.6;
+    //    givlog->debug("brain_decition_speed","dspeed: %f",
+    //                  dSpeed);
+
+    if(vehState==changingRoad)
+    {
+        if(gpsTraceNow[0].x>1.0)
+        {
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }
+        else if(gpsTraceNow[0].x<-1.0)
+        {
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }
+        else
+        {
+            vehState=normalRun;
+        }
+    }
+
+    static int obs_filter=0,obs_filter_flag=0,obs_filter_dis_memory=0;
+    static double obs_speed_for_avoid=-1,obs_filter_speed_memory=0;   //obs_speed_for_avoid: obstacle actual speed in km/h
+    if(!ServiceCarStatus.daocheMode)
+    {
+        //computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        computeObsOnRoadXY(now_gps_ins,lidarGridPtr, gpsTraceNow,gpsTraceNowLeft,gpsTraceNowRight,roadNow,gpsMapLine,lidar_per);
+        if(obs_filter_flag==0)
+        {
+            if(obsDistance>0&&obsDistance<60)
+            {
+                obs_filter++;
+                if(obs_filter<20)                          //80
+                {
+                    obsDistance=-1;
+                    obsSpeed=0;
+                }
+                else
+                {
+                    obs_filter_flag=1;
+                    obs_filter_dis_memory=obsDistance;
+                    obs_filter_speed_memory=obsSpeed;
+                    obs_filter=0;
+                }
+            }
+            else
+            {
+                obs_filter=0;
+            }
+        }
+        else
+        {
+            if(obsDistance<0||obsDistance>60)
+            {
+                obs_filter++;
+                if(obs_filter<20)                           //80
+                {
+                    obsDistance=obs_filter_dis_memory;
+                    obsSpeed=obs_filter_speed_memory;
+                }
+                else
+                {
+                    obs_filter_flag=0;
+                    obs_filter=0;
+                }
+            }
+            else
+            {
+                obs_filter=0;
+                obs_filter_dis_memory=obsDistance;
+                obs_filter_speed_memory=obsSpeed;
+            }
+        }
+
+
+        obs_speed_for_avoid=(secSpeed+obsSpeed)*3.6;
+
+        givlog->debug("decition_brain","obs_filter: %d,obs_filter_flag: %d,obs_filter_dis_memory: %f,obsDistance: %f,obsSpeed: %f,obs_speed_for_avoid: %f",
+                      obs_filter,obs_filter_flag,obs_filter_dis_memory,obsDistance,obsSpeed,obs_speed_for_avoid);
+
+        obs_speed_for_avoid=0;//shenlan guosai zhangaiwu sudu zhijiegei 0
+    }
+    else
+    {
+        gpsTraceRear.clear();
+        for(unsigned int i=0;i<gpsTraceNow.size();i++)
+        {
+            Point2D pt;
+            pt.x=0-gpsTraceNow[i].x;
+            pt.y=0-gpsTraceNow[i].y;
+            gpsTraceRear.push_back(pt);
+        }
+        computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
+        //  obsDistance=-1; //1227
+    }
+
+    ServiceCarStatus.mObsDistance=obsDistance;
+    ServiceCarStatus.mObsSpeed=obs_speed_for_avoid;
+
+    static bool avoid_speed_flag=false;
+    /*if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis+10)&&(abs(realspeed)>0.5)&&
+            (vehState==normalRun) &&(ObsTimeStart==-1)//&&(obs_speed_for_avoid<0.6)
+            && (gpsMapLine[PathPoint]->runMode==1)&&(gpsMapLine[PathPoint]->mode2!=3000))
+    {
+        minDecelerate=-0.4;
+        avoid_speed_flag=true;
+    }*/
+
+    road_permit_avoid=false;
+
+    if(ServiceCarStatus.msysparam.mvehtype=="shenlan") //比赛总路线可能不会太长
+    {
+        if(PathPoint+200<gpsMapLine.size()){
+            int road_permit_sum=0;
+            for(int k=PathPoint;k<PathPoint+200;k++){
+                if((gpsMapLine[k]->mbnoavoid==false))//&&(gpsMapLine[k]->roadSum>1))
+                    road_permit_sum++;
+            }
+            if(road_permit_sum>=200)
+                road_permit_avoid=true;
+        }
+    }
+    else
+    {
+        if(PathPoint+200<gpsMapLine.size()){   //400gaiwei 200
+            int road_permit_sum=0;
+            for(int k=PathPoint;k<PathPoint+200;k++){
+                if((gpsMapLine[k]->mbnoavoid==false))//&&(gpsMapLine[k]->roadSum>1))
+                    road_permit_sum++;
+            }
+            if(road_permit_sum>=200)
+                road_permit_avoid=true;
+        }
+    }
+
+    //shiyanbizhang 0929
+    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)&&(realspeed>0.01)//&& (avoid_speed_flag==true)        //
+            &&(vehState==normalRun||vehState==backOri || vehState==avoiding) //&&(ObsTimeStart==-1)
+            && (gpsMapLine[PathPoint]->runMode==1)&& (road_permit_avoid==true)&&(gpsMapLine[PathPoint]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=3000)&&(gpsMapLine[PathPoint+100]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=23)){
+        //        ObsTimeStart=GetTickCount();
+        ObsTimeEnd+=1.0;
+        //dSpeed = min(6.0,dSpeed);
+        cout<<"\n====================preAvoid time count start==================\n"<<endl;
+    }
+    //    if(ObsTimeStart!=-1){
+    //        ObsTimeEnd=GetTickCount();
+    //    }
+    //    if(ObsTimeEnd!=-1){
+    //        ObsTimeSpan=ObsTimeEnd-ObsTimeStart;
+    //    }
+
+
+
+    //    if(ObsTimeSpan>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
+    //        vehState=avoidObs;
+    //        ObsTimeStart=-1;
+    //        ObsTimeEnd=-1;
+    //        ObsTimeSpan=-1;
+    //        avoid_speed_flag=false;
+    //        cout<<"\n====================preAvoid time count finish==================\n"<<endl;
+    //    }
+
+
+
+
+    //    if((ObsTimeStart!=-1)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)){
+    //            ObsTimeEnd+=1.0;
+    //    }
+
+    if(ObsTimeEnd>ObsTimeWidth &&(obsDistance>1.5||obsDistance<0) && (gpsMapLine[PathPoint]->runMode==1)){ //1026
+        vehState=avoidObs;
+        ObsTimeStart=-1;
+        ObsTimeEnd=-1;
+        ObsTimeSpan=-1;
+        avoid_speed_flag=false;
+        cout<<"\n====================preAvoid time count finish==================\n"<<endl;
+    }
+
+    if((obsDistance<0 ||obsDistance>ServiceCarStatus.aocfDis)||(road_permit_avoid==false)||(front_car_decide_avoid==false)){
+        ObsTimeStart=-1;
+        ObsTimeEnd=-1;
+        ObsTimeSpan=-1;
+        avoid_speed_flag=false;
+    }
+
+    //避障模式
+    if (vehState == avoidObs   || vehState==waitAvoid ) {
+
+        //      if (obsDistance <20 && obsDistance >0)
+        if (obsDistance <ServiceCarStatus.aocfDis && obsDistance >0)
+        {
+            dSpeed = min(6.0,dSpeed);
+            avoidTimes++;
+            //          if (avoidTimes>=6)
+            if (avoidTimes>=ServiceCarStatus.aocfTimes)
+            {
+                vehState = preAvoid;
+                avoidTimes = 0;
+            }
+
+        }
+        //        else if (obsDistance < 35 && obsDistance >0 && dSpeed>10)
+        //        {
+        //            dSpeed = 10;
+        //            avoidTimes = 0;
+        //        }
+        else if (obsDistance < 40 && obsDistance >0 && dSpeed>15)
+        {
+            dSpeed =  min(15.0,dSpeed);
+            avoidTimes = 0;
+        }
+        else
+        {
+            avoidTimes = 0;
+        }
+
+    }
+
+
+    if((vehState == preAvoid)||(avoidXNew!=0.0))//jiashang avoiding fangzhi avoid=0.0buneng lianbi
+    {
+        dSpeed = min(6.0,dSpeed);
+//        int avoidLeft_value=0;
+//        int avoidRight_value=0;
+//        int* avoidLeft_p=&avoidLeft_value;
+//        int* avoidRight_p=&avoidRight_value;
+        double avoidLeft_value=0;
+        double avoidRight_value=0;
+        double* avoidLeft_p=&avoidLeft_value;
+        double* avoidRight_p=&avoidRight_value;
+        computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
+        avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,avoidLeft_value,avoidRight_value,avoidXNewLast);
+
+        givlog->debug("decition_brain","======avoidXNewPre==========:%f",avoidXNewPre);
+
+        //const int nAvoidPreCount = 100;
+        if(avoidXNewPreVector.size()<5){
+                   avoidXNewPreVector.push_back(avoidXNewPre);
+               }else{
+                   if(avoidXNewPreVector[0]!=avoidXNewLast){
+                       for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
+                           if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
+                               avoidXNewPreFilter=avoidXNewLast;
+                               break;
+                           }
+                           if(i==avoidXNewPreVector.size()-1)
+                               avoidXNewPreFilter=avoidXNewPreVector[0];
+                       }
+                   }
+                   avoidXNewPreVector.clear();
+               }
+               if(avoidXNewPreFilter!=avoidXNewLast){
+                   avoidXNew=avoidXNewPre;
+                   if(avoidXNew<0)
+                       turnLampFlag=-1;
+                   else if(avoidXNew>0)
+                       turnLampFlag=1;
+                   else
+                       turnLampFlag=0;
+
+                   gpsTraceNow.clear();
+                   gpsTraceAvoidXY.clear();
+                   givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
+                                 avoidXNew,avoidXNewLast);
+                   //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+                   gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+                   vehState = avoiding;
+                   startAvoidGpsIns = now_gps_ins;
+                   obstacle_avoid_flag=1;
+                   hasCheckedAvoidLidar = false;
+                   avoidXNewLast=avoidXNew;
+                   avoidXNewPreFilter=avoidXNew;
+               }
+           }
+
+//        if(avoidXNewPreVector.size()<nAvoidPreCount){
+//            avoidXNewPreVector.push_back(avoidXNewPre);
+//        }
+//        else
+//        {
+//            avoidXNewPreVector.erase(avoidXNewPreVector.begin());
+//            avoidXNewPreVector.push_back(avoidXNewPre);
+//        }
+
+//        if(vehState == preAvoid)
+//        {
+//            avoidXNew = avoidXNewPre;
+//        }
+//        else
+//        {
+//            if(fabs(avoidXNewLast)>0.1)
+//            {
+
+//                bool bOriginSafe = true;
+
+//                if(avoidXNewPreVector.size()<nAvoidPreCount)
+//                {
+//                    bOriginSafe = false;
+//                }
+//                else
+//                {
+//                    unsigned int j;
+//                    unsigned int nSize = avoidXNewPreVector.size();
+//                    for(j=0;j<nSize;j++)
+//                    {
+//                        if(fabs(avoidXNewPreVector[j])>0.1)
+//                        {
+//                            bOriginSafe = false;
+//                            break;
+//                        }
+//                    }
+//                }
+//                if(bOriginSafe)
+//                {
+//                    avoidXNew = 0;
+// //                   avoidXNewPreVector.clear();
+//                }
+//                else
+//                {
+//                    unsigned int j;
+//                    unsigned int nSize = avoidXNewPreVector.size();
+//                    if(avoidXNewPreVector.size()==nAvoidPreCount)
+//                    {
+//                        double filter = 0;
+//                        for(j=0;j<nSize;j++)
+//                        {
+//                            if(fabs(avoidXNewPreVector[j])>0.1)
+//                            {
+//                                if(fabs(filter)<0.1)filter = avoidXNewPreVector[j];
+//                                else
+//                                {
+//                                    if(fabs(filter - avoidXNewPreVector[j])>0.1)
+//                                    {
+//                                        filter = 0;
+//                                        break;
+//                                    }
+//                                }
+//                            }
+//                        }
+//                        if(fabs(filter)<0.1)
+//                        {
+//                            avoidXNew = avoidXNewLast;
+//                        }
+//                        else
+//                        {
+//                            if(fabs(filter - avoidXNewLast)>=2.0)
+//                            {
+//                                avoidXNew = filter;
+//                            }
+//                            else
+//                            {
+//                                avoidXNew = avoidXNewLast;
+//                            }
+//                        }
+//                    }
+//                    else
+//                    {
+//                        avoidXNew = avoidXNewLast;
+//                    }
+//                }
+//            }
+//        }
+
+
+//        if(avoidXNew<0)
+//            turnLampFlag=-1;
+//        else if(avoidXNew>0)
+//            turnLampFlag=1;
+//        else
+//            turnLampFlag=0;
+
+//        if(fabs(avoidXNew - avoidXNewLast)>0.1)
+//        {
+//            gpsTraceNow.clear();
+//            gpsTraceAvoidXY.clear();
+//            givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
+//                          avoidXNew,avoidXNewLast);
+//            //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+//            gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+//            vehState = avoiding;
+//            startAvoidGpsIns = now_gps_ins;
+//            obstacle_avoid_flag=1;
+//            hasCheckedAvoidLidar = false;
+//            avoidXNewLast=avoidXNew;
+//        }
+
+//        if(avoidXNewPreVector.size()<5){
+//            avoidXNewPreVector.push_back(avoidXNewPre);
+//        }else{
+//            if(avoidXNewPreVector[0]!=avoidXNewLast){
+//                for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
+//                    if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
+//                        avoidXNewPreFilter=avoidXNewLast;
+//                        break;
+//                    }
+//                    if(i==avoidXNewPreVector.size()-1)
+//                        avoidXNewPreFilter=avoidXNewPreVector[0];
+//                }
+//            }
+//            avoidXNewPreVector.clear();
+//        }
+//        if(avoidXNewPreFilter!=avoidXNewLast){
+//            avoidXNew=avoidXNewPre;
+//            if(avoidXNew<0)
+//                turnLampFlag=-1;
+//            else if(avoidXNew>0)
+//                turnLampFlag=1;
+//            else
+//                turnLampFlag=0;
+
+//            gpsTraceNow.clear();
+//            gpsTraceAvoidXY.clear();
+//            givlog->debug("decition_brain","avoidXNew: %f,avoidXNewLast: %f",
+//                          avoidXNew,avoidXNewLast);
+//            //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+//            gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+//            vehState = avoiding;
+//            startAvoidGpsIns = now_gps_ins;
+//            obstacle_avoid_flag=1;
+//            hasCheckedAvoidLidar = false;
+//            avoidXNewLast=avoidXNew;
+//            avoidXNewPreFilter=avoidXNew;
+//        }
+//    }
+//20230407,
+//    if((vehState == preAvoid)||((avoidXNew!=0)))
+//  //    if((vehState == preAvoid)||(avoidXNew!=0))
+//      {
+//          dSpeed = min(8.0,dSpeed);//6gaiwei 10
+//          static int count_avoidx_0=0;
+//          double avoidLeft_value=0;
+//          double avoidRight_value=0;
+//          double* avoidLeft_p=&avoidLeft_value;
+//          double* avoidRight_p=&avoidRight_value;
+//          computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
+//         // avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value,avoidXNewLast);
+//          avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,avoidLeft_value,avoidRight_value,avoidXNewLast);
+//          if(avoidXNewPreVector.size()<20){
+//              avoidXNewPreVector.push_back(avoidXNewPre);
+//          }else{
+//              if(avoidXNewPreVector[0]!=avoidXNewLast){
+//                  for(unsigned int i=1;i<avoidXNewPreVector.size();i++){
+//                      if(avoidXNewPreVector[i]!=avoidXNewPreVector[0]){
+//                          avoidXNewPreFilter=avoidXNewLast;
+//                          break;
+//                      }
+//                      if(i==avoidXNewPreVector.size()-1)
+//                          avoidXNewPreFilter=avoidXNewPreVector[0];
+//                  }
+//              }
+//              avoidXNewPreVector.clear();
+//          }
+//          if(avoidXNewPreFilter!=avoidXNewLast){
+//              avoidXNew=avoidXNewPre;
+//             //avoidXNew = avoidXNewPreFilter;
+//              if(avoidXNew<0)
+//                  turnLampFlag=-1;
+//              else if(avoidXNew>0)
+//                  turnLampFlag=1;
+//              else
+//                  turnLampFlag=0;
+
+//  //           gpsTraceNow.clear();
+////             gpsTraceAvoidXY.clear();
+//             givlog->debug("decition_brain","avoidXNew: %d,avoidXNewLast: %d",
+//                             avoidXNew,avoidXNewLast);
+//             //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
+//  //           gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+//             if(vehState == preAvoid)
+//             {
+//                 gpsTraceAvoidXY.clear();
+//                 gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+//             }
+//             else if(avoidXNew==0)
+//             {
+//                 count_avoidx_0++;
+//                 if(count_avoidx_0>45)// you 60 gaicheng 30
+//                 {
+//                     gpsTraceAvoidXY.clear();
+//                    gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+
+//                 }
+//              }
+//             if(avoidXNew!=0)
+//             {
+//                 count_avoidx_0=0;
+//             }
+//             vehState = avoiding;
+//             startAvoidGpsIns = now_gps_ins;
+//             obstacle_avoid_flag=1;
+//             hasCheckedAvoidLidar = false;
+//             avoidXNewLast=avoidXNew;
+//             avoidXNewPreFilter=avoidXNew;
+//          }
+//      }
+//  if (vehState == preAvoid)
+//  {
+//      //dSpeed = min(6.0,dSpeed);
+//      iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);
+//              if ((obsDistance>ServiceCarStatus.aocfDis)&&(fabs(now_s_d.d)<0.05))
+//              {
+//                  // vehState = avoidObs; 0929
+//                  vehState=normalRun;
+//              }
+//  }
+//
+
+    //old_bz--------------------------------------------------------------------------------------------------
+    if (vehState == avoiding)
+    {
+        //        double avoidDistance=GetDistance(startAvoidGpsIns,now_gps_ins);   //Toggle 20220223
+        //        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
+        //        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
+        //            vehState = normalRun;
+        //            //            useFrenet = false;
+        //            //            useOldAvoid = true;
+        //        }
+        //        else if (useOldAvoid && avoidDistance>35) {    //zj 2021.06.21   gpsTraceNow[60].x)<0.02
+        //            // vehState = avoidObs; 0929
+        //            vehState = normalRun;
+        //            turnLampFlag=0;
+        //        }
+        //        else if (turnLampFlag>0)
+        //        {
+        //            gps_decition->leftlamp = false;
+        //            gps_decition->rightlamp = true;
+        //        }
+        //        else if(turnLampFlag<0)
+        //        {
+        //            gps_decition->leftlamp = true;
+        //            gps_decition->rightlamp = false;
+        //        }
+
+        dSpeed = min(6.0,dSpeed);
+        if (turnLampFlag>0)
+        {
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }
+        else if(turnLampFlag<0)
+        {
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }
+    }
+
+
+    if (vehState==preBack)
+    {
+        vehState = backOri;
+    }
+
+    if (vehState==backOri)
+    {
+        double backDistance=GetDistance(startBackGpsIns,now_gps_ins);
+        //若车辆到达变道后的路径，改变车辆状态，关闭frenet规划
+        if (useFrenet && abs(gpsTraceAim[0].x)<1.0) {
+            vehState = normalRun;
+            turnLampFlag=0;
+            //            useFrenet = false;
+            //            useOldAvoid = true;
+        }
+        else if (useOldAvoid && backDistance>35 ) {//abs(gpsTraceNow[60].x)<0.05
+            // vehState = avoidObs; 0929
+            vehState = normalRun;
+            obstacle_avoid_flag=0;
+        }
+        else if (turnLampFlag>0)
+        {
+            gps_decition->leftlamp = false;
+            gps_decition->rightlamp = true;
+        }
+        else if (turnLampFlag<0)
+        {
+            gps_decition->leftlamp = true;
+            gps_decition->rightlamp = false;
+        }
+    }
+
+
+    std::cout<<"\n原始RoadOri:%d\n"<<roadOri<<endl;
+    //   计算回归原始路线
+#ifdef AVOID_NEW
+    if((avoidXNew == 0.0)&&(vehState == avoiding))
+    {
+        iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);
+        if(fabs(now_s_d.d)<0.05)
+        {
+            vehState=normalRun;
+            obstacle_avoid_flag=0;
+           // avoidXNewPreVector.clear();//20230526,huifu
+        }
+    }
+
+
+#else
+    if ((roadNow!=roadOri))
+    {
+        if(useFrenet){
+            if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],30.0)))
+            {
+                double offsetL = -(roadSum - 1)*roadWidth;
+                double offsetR = (roadOri - 0)*roadWidth;
+                roadPre = frenetPlanner->chooseRoadByFrenet(now_gps_ins,gpsMapLine,PathPoint,offsetL,offsetR,lidarGridPtr,roadNow);
+            }
+        }
+        else if(useOldAvoid){
+            roadPre = chooseBackRoad(lidarGridPtr, now_gps_ins, gpsMapLine,lidar_per);
+            //  avoidX = (roadOri - roadNow)*roadWidth; //1012
+            avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint]);
+        }
+    }
+    if ((roadNow != roadOri && roadPre!=-1))
+    {
+
+        roadNow = roadPre;
+        //     avoidX = (roadOri - roadNow)*roadWidth;
+        avoidX=computeAvoidX(roadNow,roadOri,gpsMapLine[PathPoint]);
+
+        if(avoidX<0)
+            turnLampFlag<0;
+        else if(avoidX>0)
+            turnLampFlag>0;
+        else
+            turnLampFlag=0;
+
+        if(useOldAvoid){
+            //gpsTraceNow = getGpsTraceOffset(gpsTraceOri, avoidX);
+            gpsTraceAvoidXY.clear();
+            gpsTraceAvoidXY = getGpsTraceOffsetBackOri(gpsTraceOri, avoidX,now_gps_ins,gpsTraceNow);
+            startBackGpsIns = now_gps_ins;
+        }
+        else if(useFrenet){
+            gpsTraceNow = frenetPlanner->getPath(now_gps_ins,gpsMapLine,PathPoint,avoidX,lidarGridPtr);
+        }
+
+        vehState = backOri;
+        hasCheckedBackLidar = false;
+        //  checkBackObsTimes = 0;
+
+        cout<<"\n回归RoadNum:%d\n"<< roadNow<<endl;
+
+    }
+#endif
+
+    if ( vehState==changingRoad || vehState==chaocheBack)
+    {
+        double lastAng = 0.0 - lastAngle;
+
+        if(ServiceCarStatus.msysparam.mvehtype !="zhongche"){
+
+            if (controlAng>40)
+            {
+                controlAng =40;
+            }
+            else if (controlAng<-40)
+            {
+                controlAng = - 40;
+            }
+        }
+    }
+
+
+    //速度结合角度的限制
+    controlAng = limitAngle(realspeed, controlAng);
+
+    //1220
+    if(PathPoint+80<gpsMapLine.size()-1){
+        if(gpsMapLine[PathPoint+80]->roadMode==4  && !ServiceCarStatus.daocheMode){
+            changingDangWei=true;
+        }
+    }
+
+    if(changingDangWei){
+        if(abs(realspeed)>0.1){
+            dSpeed=0;
+        }else{
+            dSpeed=0;
+            gps_decition->dangWei=2;
+            ServiceCarStatus.daocheMode=true;
+        }
+    }
+
+    if(ServiceCarStatus.daocheMode)
+    {
+        controlAng=0-controlAng;
+    }
+
+
+    dSecSpeed = dSpeed / 3.6;
+    gps_decition->speed = dSpeed;
+
+    if (gpsMapLine[PathPoint]->runMode == 2)
+    {
+        obsDistance = -1;
+
+    }
+
+
+    //----------------------------------------shenlan 采集车车路协同,add---------------------------------------------
+    if(ServiceCarStatus.mRSUupdateTimer.elapsed()>10*1000)
+     {
+        ServiceCarStatus.rsu_traffic_type=200;//lvdeng
+        ServiceCarStatus.rsu_warning_type=200;
+ //       ServiceCarStatus.rsu_gps_lat = 0.0;  //路况信息或预警信息有效的时候才会使用到这写变量，所以这些变量的信息可不清除
+ //       ServiceCarStatus.rsu_gps_lng = 0.0;
+ //       ServiceCarStatus.rsu_trafficelimit_spd=200;
+     }
+     if(ServiceCarStatus.mRSUTrafficUpdateTimer.elapsed()>10*1000)
+     {
+        ServiceCarStatus.rsu_traffic_type=200;//lvdeng
+        ServiceCarStatus.rsu_trafficelimit_spd=200;
+
+//        ServiceCarStatus.iTrafficeLightLon=0;//20230310
+//        ServiceCarStatus.iTrafficeLightLat=0;//20230310
+
+ //       ServiceCarStatus.rsu_warning_type=200;
+ //       ServiceCarStatus.rsu_gps_lat = 0.0;  //路况信息或预警信息有效的时候才会使用到这写变量，所以这些变量的信息可不清除
+ //       ServiceCarStatus.rsu_gps_lng = 0.0;
+ //       ServiceCarStatus.rsu_trafficelimit_spd=200;
+     }
+     if(ServiceCarStatus.mRSUWarnUpdateTimer.elapsed()>10*1000)
+     {
+ //       ServiceCarStatus.rsu_traffic_type=200;//lvdeng
+        ServiceCarStatus.rsu_warning_type=200;
+        ServiceCarStatus.rsu_warnlimit_spd=200;
+ //       ServiceCarStatus.rsu_gps_lat = 0.0;  //路况信息或预警信息有效的时候才会使用到这写变量，所以这些变量的信息可不清除
+ //       ServiceCarStatus.rsu_gps_lng = 0.0;
+ //       ServiceCarStatus.rsu_trafficelimit_spd=200;
+     }
+    unsigned char traffic_type=ServiceCarStatus.rsu_traffic_type;//路况信息
+    unsigned char warning_type=ServiceCarStatus.rsu_warning_type;//预警信息，RSU获得
+    float distance_to_center=0;
+    GPS_INS traffic_center_gps;
+    traffic_center_gps.gps_lat=ServiceCarStatus.rsu_gps_lat;
+    traffic_center_gps.gps_lng=ServiceCarStatus.rsu_gps_lng;
+    float radiation_distance=ServiceCarStatus.rsu_radiation_distance;//事件辐射范围，从RSU获得
+    float trafficlimit_spd=ServiceCarStatus.rsu_trafficelimit_spd; //从RSU获得的限速值
+    float warnlimit_spd=ServiceCarStatus.rsu_warnlimit_spd; //从RSU获得的限速值
+    //以上变量信息都需要存储到log文件中
+    GaussProjCal(traffic_center_gps.gps_lng, traffic_center_gps.gps_lat,  &traffic_center_gps.gps_x, &traffic_center_gps.gps_y);
+   distance_to_center=GetDistance(now_gps_ins,traffic_center_gps);//因为是长直道路所以直接计算事件中心点和当前位置的距离即可
+
+   if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03)||(traffic_type==0x04)
+        ||(warning_type==0x01)||(warning_type==0x02))
+{
+   if((traffic_type==0x01)||(traffic_type==0x02)||(traffic_type==0x03))//塌方，施工，道路结冰，事件范围外10米缓慢减速，事件范围内停车
+    {
+       if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+10))
+       {
+          //dSpeed = min(1.0,realspeed-0.2);
+          dSpeed = max(1.0,realspeed-2);  //乘用车不同于底盘，速度是不稳定的，如果取最小一会就减成0了
+          ServiceCarStatus.vehicle_state_1x = 1;
+          ServiceCarStatus.target_spd_1x = 1.0;
+       }
+       else if(distance_to_center<radiation_distance)
+       {
+           dSpeed=0.0;
+           minDecelerate=-2.0;
+           ServiceCarStatus.vehicle_state_1x = 2;
+           ServiceCarStatus.target_spd_1x=0.0;
+       }
+       else
+       {}
+       if(ServiceCarStatus.txt_log_on==true)
+       {
+           givlog->debug("decition_brain","traffic_center_gps.gps_lat is : %f",traffic_center_gps.gps_lat);
+           givlog->debug("decition_brain","traffic_center_gps.gps_lng is : %f",traffic_center_gps.gps_lng);
+           givlog->debug("decition_brain","distance_to_center is : %f",distance_to_center);
+           givlog->debug("decition_brain","radiation_distance is : %f",radiation_distance);
+           givlog->debug("decition_brain","dSpeedd is : %f",dSpeed);
+           givlog->debug("decition_brain","minDecelerate is : %f",minDecelerate);
+           givlog->debug("decition_brain","traffic_type is : %d",traffic_type);
+       }
+    }
+   else if(traffic_type==0x04)//如果事件类型是限速，则在辐射范围内限速，辐射范围外正常行驶
+    {
+       if((distance_to_center>=radiation_distance)&&(distance_to_center<=radiation_distance+2))
+       {
+//          dSpeed=min((double)trafficlimit_spd,realspeed-0.5);//先让速度稍微减少一点
+          if(realspeed-2>trafficlimit_spd)
+           {
+                dSpeed=realspeed-2;//先让速度稍微减少一点
+           }
+           ServiceCarStatus.vehicle_state_1x = 1;
+           ServiceCarStatus.target_spd_1x = trafficlimit_spd;
+       }
+       else if(distance_to_center<radiation_distance)
+       {
+          //dSpeed=min((double)trafficlimit_spd,realspeed);
+           dSpeed=(double)trafficlimit_spd;
+           ServiceCarStatus.vehicle_state_1x = 1;
+           ServiceCarStatus.target_spd_1x = trafficlimit_spd;
+       }
+       else
+       {}
+       if(ServiceCarStatus.txt_log_on==true)
+       {
+           givlog->debug("decition_brain","distance_to_center is : %f",distance_to_center);
+           givlog->debug("decition_brain","radiation_distance is : %f",radiation_distance);
+           givlog->debug("decition_brain","dSpeed is : %f",dSpeed);
+           givlog->debug("decition_brain","traffic_type is : %d",traffic_type);
+       }
+    }
+    else
+    {}
+//碰撞预警，1减速，2 停车
+    if(warning_type==0x01)
+    {
+        dSpeed=warnlimit_spd;//此处要判断限速值是不是在有效值范围以内
+        ServiceCarStatus.vehicle_state_1x = 1;
+        ServiceCarStatus.target_spd_1x = warnlimit_spd;
+    }
+    else if(warning_type==0x02)
+    {
+        dSpeed=0.0;
+         ServiceCarStatus.vehicle_state_1x = 2;
+         ServiceCarStatus.target_spd_1x = 0;
+    }
+    else
+    {}
+}
+else
+{
+    ServiceCarStatus.vehicle_state_1x = 0;
+    ServiceCarStatus.target_spd_1x = dSpeed;
+}
+
+    std::cout<<"juecesudu0="<<dSpeed<<std::endl;
+
+    //ServiceCarStatus.milightCheckTimer.elapsed();
+    if(ServiceCarStatus.milightCheckTimer.elapsed()>5*1000)
+    {
+       ServiceCarStatus.iTrafficeLightColor=1;//lvdeng
+       ServiceCarStatus.iTrafficeLightTime=200;
+    }
+
+    //-------------------------------traffic light----------------------------------------------------------------------------------------
+
+    //1125 traficc
+    traffic_light_gps.gps_lat=ServiceCarStatus.iTrafficeLightLat;
+    traffic_light_gps.gps_lng=ServiceCarStatus.iTrafficeLightLon;
+    GaussProjCal(traffic_light_gps.gps_lng,traffic_light_gps.gps_lat, &traffic_light_gps.gps_x, &traffic_light_gps.gps_y);
+
+    if(traffic_light_gps.gps_lat!=0 && traffic_light_gps.gps_lng!=0){
+        traffic_light_pathpoint = Compute00().getNoAngleNearestPointIndex(traffic_light_gps, gpsMapLine);
+        //    traffic_light_pathpoint=130;
+        //float traffic_speed = ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,gpsMapLine, traffic_light_pathpoint,PathPoint,secSpeed);
+        float traffic_speed=ComputeTrafficLightSpeed(ServiceCarStatus.iTrafficeLightColor,ServiceCarStatus.iTrafficeLightTime,gpsMapLine,
+                                                             traffic_light_pathpoint ,PathPoint,secSpeed,dSpeed);
+
+        dSpeed = min((double)traffic_speed,dSpeed);
+        if(traffic_speed==0)
+        {
+            minDecelerate=-2.0; //-2.0 ,深蓝车速比较小将减速度改小一些
+        }
+        if(ServiceCarStatus.txt_log_on==true)
+        {
+           givlog->debug("decition_brain","traffic_light_pathpoint is : %d",traffic_light_pathpoint);
+           givlog->debug("decition_brain","traffic_speed is : %f",traffic_speed);
+           givlog->debug("decition_brain","traffic_light_gps.gps_lat is : %f",traffic_light_gps.gps_lat);
+           givlog->debug("decition_brain","traffic_light_gps.gps_lng is : %f",traffic_light_gps.gps_lng);
+           givlog->debug("decition_brain","ServiceCarStatus.iTrafficeLightColor is : %d",ServiceCarStatus.iTrafficeLightColor);
+           givlog->debug("decition_brain","ServiceCarStatus.iTrafficeLightTime is : %d",ServiceCarStatus.iTrafficeLightTime);
+        }
+    }
+    //-------------------------------traffic light  end-----------------------------------------------------------------------------------------------
+
+
+    //-------------------------------v2x traffic light-----------------------------------------------------------------------------------------
+
+
+    double v2xTrafficSpeed= ComputeV2XTrafficLightSpeed( trafficLight,  gpsMapLine, v2xTrafficVector,
+                                                         PathPoint, secSpeed, dSpeed,  circleMode);
+
+
+    dSpeed = min(v2xTrafficSpeed,dSpeed);
+
+    //------------------------------v2x traffic light end--------------------------------------------------------------------------------------
+
+    //20230310 add begin,shanlan 由决策决定什么时候开启红绿等检测
+    float distance_to_stopline;
+    distance_to_stopline=GetDistance(now_gps_ins,traffic_light_gps);
+    givlog->debug("decition_brain","distance_to_stopline is : %f",distance_to_stopline);
+    if(distance_to_stopline<20.0)
+    {
+        ServiceCarStatus.mLightStartSensorBtn=true;
+
+    }
+    else
+    {
+        ServiceCarStatus.mLightStartSensorBtn=false;
+    }
+    //20230310 add end,由决策决定什么时候开启红绿等检测
+
+    transferGpsMode2(gpsMapLine);
+/**************************zhuche/tingche logic begin**************************************************************/
+    //准备驻车
+    if (readyZhucheMode)
+    {
+        cout<<"ready ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
+        cout<<"zhuche point : "<<zhuchePointNum<<endl;
+        double dis = GetDistance(*gpsMapLine[zhuchePointNum], now_gps_ins);
+        if (dis<35)
+        {
+            Point2D pt = Coordinate_Transfer((*gpsMapLine[zhuchePointNum]).gps_x, (*gpsMapLine[zhuchePointNum]).gps_y, now_gps_ins);
+            double zhucheDistance = pt.y;
+#if 0
+            if (dSpeed > 15)
+            {
+                dSpeed = 15;
+            }
+
+            if (zhucheDistance < 20 && dis < 25)
+            {
+                dSpeed = min(dSpeed, 5.0);
+            }
+#else
+            if (dSpeed > 12)
+            {
+                dSpeed = 12;
+            }
+
+            if (zhucheDistance < 9 && dis < 9)
+            {
+                dSpeed = min(dSpeed, 5.0);
+            }
+#endif
+            if (zhucheDistance < 3.0 && dis < 3.5)
+            {
+                dSpeed = min(dSpeed, 5.0);
+                zhucheMode = true;
+                readyZhucheMode = false;
+                cout<<"jinru ZHECHE STATE+++++++++++++++++++++++++++++++++"<<endl;
+            }
+        }
+    }
+    //驻车
+    if (zhucheMode)
+    {
+        int mzcTime = ServiceCarStatus.msysparam.mzhuchetime * 1000;
+
+        //        if(trumpet()<16000)
+        if (trumpet()<mzcTime)
+        {
+            //            if (trumpet()<12000){
+            cout<<"ZHECHE ING STATE+++++++++++++++++++++++++++++++++"<<endl;
+            minDecelerate=-1.0;
+            if(abs(realspeed)<0.2){
+                controlAng=0;  //tju
+            }
+        }
+        else
+        {
+            hasZhuched = true; //1125
+            zhucheMode = false;
+            trumpetFirstCount = true;
+            cout<<"ZHECHE STATE JIESHU +++++++++++++++++++++++++++++++++"<<endl;
+        }
+    }
+    //判断驻车标志位
+    if (hasZhuched && GetDistance(now_gps_ins,*gpsMapLine[zhuchePointNum])>20)
+    {
+        hasZhuched = false;
+    }
+
+    if (readyParkMode)
+    {
+        double parkDis = GetDistance(now_gps_ins, *gpsMapLine[finishPointNum]);
+        hasZhuched = true;
+
+        if (parkDis < 25)
+        {
+            Point2D pt = Coordinate_Transfer((*gpsMapLine[finishPointNum]).gps_x, (*gpsMapLine[finishPointNum]).gps_y, now_gps_ins);
+            double parkDistance = pt.y;
+            if (dSpeed > 15)
+            {
+                dSpeed = 15;
+            }
+
+            //dSpeed = 15;//////////////////////////////
+
+            if (parkDistance < 15 && parkDistance >= 4.5 && parkDis<20)
+            {
+                dSpeed = min(dSpeed, 8.0);
+            }
+            else if (parkDistance < 4.5 && parkDistance >= 3.5 && parkDis<5.0)
+            {
+                dSpeed = min(dSpeed, 5.0);
+            }
+            else if(parkDistance < 3.5 && parkDis<4.0)
+            {
+                dSpeed = min(dSpeed, 3.0);
+            }
+
+            //            float stopDis=2;
+            //            if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
+            //                stopDis=1.6;
+            //            } else if(ServiceCarStatus.msysparam.mvehtype=="vv7"){
+            //                stopDis=1.8;
+            //            }
+
+            if (parkDistance < 1.6  && parkDis<2.0)
+            {
+                // dSpeed = 0;
+                parkMode = true;
+                readyParkMode = false;
+            }
+        }
+    }
+
+    if (parkMode)
+    {
+        minDecelerate=-1.0;
+
+        if(finishPointNum<0||finishPointNum>=gpsMapLine.size()){
+            parkMode=false;
+        }else if(GetDistance(now_gps_ins, *gpsMapLine[finishPointNum])>20){
+            parkMode=false;
+        }
+
+    }
+
+/**************************zhuche/tingche logic end**************************************************************/
+
+    for(int i = 0;i<ServiceCarStatus.car_stations.size();i++)
+    {
+        GPS_INS gpsIns;
+        GaussProjCal(ServiceCarStatus.car_stations[i].longtitude, ServiceCarStatus.car_stations[i].latitude,  &gpsIns.gps_x, &gpsIns.gps_y);
+
+        double dis = GetDistance(gpsIns, now_gps_ins);
+        if(dis<20)
+            ServiceCarStatus.currentstation = ServiceCarStatus.car_stations[i].ID;
+    }
+/**************************huche begin********************************************************************/
+    std::cout << "\n呼车指令状态:%d\n" <<  ServiceCarStatus.carState << std::endl;
+
+    if (ServiceCarStatus.carState == 0 && busMode)
+    {
+        minDecelerate=-1.0;
+    }
+
+    if (ServiceCarStatus.carState == 3 && busMode)
+    {
+        if(realspeed<0.1){
+            ServiceCarStatus.carState=0;
+            minDecelerate=-1.0;
+        }else{
+            nearStation=true;
+            dSpeed=0;
+        }
+    }
+
+    ///kkcai, 2020-01-03
+    //if (ServiceCarStatus.carState == 2 && busMode)
+    if (ServiceCarStatus.carState == 2)
+    {
+        aim_gps_ins.gps_lat=ServiceCarStatus.amilat;
+        aim_gps_ins.gps_lng=ServiceCarStatus.amilng;
+        //   gps2xy(ServiceCarStatus.amilat, ServiceCarStatus.amilng, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+        GaussProjCal( aim_gps_ins.gps_lng, aim_gps_ins.gps_lat,  &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
+        std::cout<<"lng"<<ServiceCarStatus.amilng<<std::endl;
+        qDebug("lat:%f", aim_gps_ins.gps_lat);
+        qDebug("lon:%f", aim_gps_ins.gps_lng);
+
+        std::cout<<"lat"<<ServiceCarStatus.amilat<<std::endl;
+        double dis = GetDistance(aim_gps_ins, now_gps_ins);
+        Point2D pt = Coordinate_Transfer((aim_gps_ins).gps_x, (aim_gps_ins).gps_y, now_gps_ins);
+
+        std::cout << "\n呼车dis距离:%f\n" << dis << std::endl;
+        std::cout << "\n呼车Y距离:%f\n" << pt.y << std::endl;
+
+        //         if (dis<20 && pt.y<8&& realspeed<0.1)
+        if (dis<20 && pt.y<5 && abs(pt.x)<3)
+        {
+            dSpeed = 0;
+            nearStation=true;
+            //is_arrivaled = true;
+            //ServiceCarStatus.status[0] = true;
+            ServiceCarStatus.istostation=1;
+            minDecelerate=-1.0;
+        }
+
+        else if (dis<20 && pt.y<15 && abs(pt.x)<3)
+        {
+            nearStation=true;
+            dSpeed = min(8.0, dSpeed);
+        }
+        else if (dis<30 && pt.y<20 && abs(pt.x)<3)
+        {
+            dSpeed = min(15.0, dSpeed);
+        }
+
+        else if (dis<50 && abs(pt.x)<3)
+        {
+            dSpeed = min(20.0, dSpeed);
+        }
+    }
+
+   /**************************huche end********************************************************************/
+    if(ServiceCarStatus.msysparam.mvehtype=="hapo")
+    {
+        if(obsDistance>0 && obsDistance<8)
+        {
+            dSpeed=0;
+        }
+    }
+//    else if(obsDistance>0 && obsDistance<15)
+    else if(obsDistance>0 && obsDistance<  ServiceCarStatus.socfDis)
+    {
+        dSpeed=0;
+    }
+
+    if(ServiceCarStatus.group_control){
+        dSpeed = ServiceCarStatus.group_comm_speed;
+    }
+
+    if(dSpeed==0){
+        if(realspeed<6)
+            minDecelerate=min(-0.5f,minDecelerate);
+        else
+            minDecelerate=min(-0.6f,minDecelerate); //1.0f    zj-0.6
+    }
+
+
+
+    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+    {
+        if(acc_end<0)
+        {
+            if(minDecelerate > acc_end) minDecelerate = acc_end;
+        }
+    }
+
+    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+    {
+        if(dSpeed>20.0)
+        {
+            dSpeed=20.0;   //shenlan bisai xiansu 10
+        }
+    }
+
+    std::cout<<"juecesudu0="<<dSpeed<<std::endl;
+
+    if(obsDistance == 0)obsDistance = -1;
+
+    phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
+    gps_decition->wheel_angle = 0.0 - controlAng;
+
+    Point2D now_s_d=iv::decition::cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
+
+    givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f,obs_speed_for_avoid: %f,mode2: %d,now_s: %f,now_d: %f",
+                  vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle,obs_speed_for_avoid,gpsMapLine[PathPoint]->mode2,now_s_d.s,now_s_d.d);
+
+    if(ServiceCarStatus.txt_log_on==true){
+        QDateTime dt = QDateTime::currentDateTime();
+        //将数据写入文件开始
+        ofstream outfile;
+        outfile.open("control_log.txt", ostream::app);
+        outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()<<'\t'
+              <<vehState<<'\t'<<PathPoint<<'\t'<<setprecision(4)<<dSpeed<<'\t'<<obsDistance<<'\t'<<obsSpeed<<'\t'<<realspeed<<'\t'<<minDecelerate<<'\t'
+             <<gps_decition->torque<<'\t'<<gps_decition->brake<<'\t'<<gps_decition->wheel_angle<<'\t'<<now_gps_ins.gps_x<<'\t'<<now_gps_ins.gps_y<<'\t'<<now_s_d.s<<'\t'<<now_s_d.d<<endl;
+        outfile.close();
+        //将数据写入文件结束
+    }
+    //shuju cunchu gognnenng add,20210624,cxw
+    if(ServiceCarStatus.txt_log_on==true)
+    {
+        if(first_start_en)
+         {
+            first_start_en=false;
+            start_tracepoint = PathPoint;
+            if(circleMode)
+            {
+                if(start_tracepoint==0)
+                {
+
+                    end_tracepoint =gpsMapLine.size()-1;  //这种计算终点坐标的序号只适合与闭环地图
+                }
+                else
+                {
+                    end_tracepoint=start_tracepoint-1;
+                }
+            }
+            else
+            {
+               end_tracepoint =gpsMapLine.size()-1; //20210929,不是闭环地图的时候终点参考坐标就是地图数组的最大序号
+            }
+         }
+         double dis1 =  GetDistance(now_gps_ins, *gpsMapLine[start_tracepoint]);
+         double dis2 = 10;// GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
+         if(brake_mode==true)
+         {
+             dis2=2;
+         }
+
+         //double dis2 =  GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
+         if(circleMode)//闭环地图
+         {
+             if(dis1<3&&dis2<3&&circleMode) //距离不能太小，太小了可能会错过，所以适当将距离增大一些
+            // if(dis1<1&&dis2<1&&circleMode)
+             {
+                 file_cnt_add_en=true;  //201200102
+             }
+             else
+             {
+                 file_cnt_add_en=false;
+             }
+
+             if(dis1>10)
+             {
+                file_cnt_add=true;
+             }
+
+             if(file_cnt_add_en&&file_num<256&&file_cnt_add)
+             {
+                 file_num++;
+                 file_cnt_add=false;
+                 map_start_point=true;
+             }
+             if(file_num==256)
+             {
+                 file_num=1;
+             }
+             else
+             {
+             }
+             QDateTime dt=QDateTime::currentDateTime();
+             QString date =dt.toString("yyyy_MM_dd_hhmm");
+             QString strsen = "1xdatalog_";
+             date.append(strsen);
+             QString s = QString::number(file_num);
+             date.append(s);
+             date.append(".txt");
+             string filename;
+             filename=date.toStdString();
+             ofstream outfile;
+             outfile.open(filename, ostream::app);
+             if((file_cnt_add==false)&&(map_start_point==true))
+             {
+                 QDateTime dt=QDateTime::currentDateTime();
+                 outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"
+                        <<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()
+                        <<endl;
+                 outfile<<"***********************the vehicle at map start point!*************************"<<endl;
+                 outfile<<"the number of lap  is "<<":    "<<file_num<<""<<endl;
+                 map_start_point=false;
+             }
+             QDateTime dt2=QDateTime::currentDateTime();
+             outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
+                    <<"Decide_Dspeed"  << ","  <<setprecision(2)<<dSpeed<< ","
+                    <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
+                    <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
+                    <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
+                    <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
+                    <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
+                    <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
+                    <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
+                    <<"Trace_Point"<< ","<<PathPoint<< ","
+                    <<"OBS_Distance"<< ","<<obsDistance<< ","
+                    <<"OBS_Speed"<< ","<<obsSpeed<< ","
+                    <<endl;
+             outfile.close();
+         }
+         else //fei yuanxing luxian
+         {
+             if(dis1<3)
+             {
+                 file_cnt_add_en=true;  //201200102
+             }
+             else
+             {
+                 file_cnt_add_en=false;
+             }
+
+             if(dis1>3)
+             {
+                file_cnt_add=true;
+             }
+
+             if(file_cnt_add_en&&file_num<256&&file_cnt_add)
+             {
+                 file_num++;
+                 file_cnt_add=false;
+                 map_start_point=true;
+             }
+             if(file_num==256)//20210712
+             {
+                 file_num=1;
+             }
+             else
+             {
+             }
+             QDateTime dt=QDateTime::currentDateTime();
+             QString date =dt.toString("yyyy_MM_dd_hhmm");
+             QString strsen = "datalog_";
+             date.append(strsen);
+             QString s = QString::number(file_num);
+             date.append(s);
+             date.append(".txt");
+             string filename;
+             filename=date.toStdString();
+             ofstream outfile;
+             outfile.open(filename, ostream::app);
+             if((file_cnt_add==false)&&(map_start_point==true))
+             {
+                 QDateTime dt=QDateTime::currentDateTime();
+                 outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()
+                       <<endl;
+                 outfile<<"********the vehicle near map start point!********"<<endl;
+                   outfile<<"the number of lap  is "<<":" <<file_num<<""<<endl;
+                 map_start_point=false;
+             }
+             if(dis2<3)
+             {
+                 outfile<<"********the vehicle near map end point!********" <<endl;
+                 QDateTime dt=QDateTime::currentDateTime();
+                 outfile<<dt.date().year()<<"-"<<dt.date().month()<<"-"<<dt.date().day()<<" "<<dt.time().hour()<<":"<<dt.time().minute()<<":"<<dt.time().second()<<"-"<<dt.time().msec()
+                       <<endl;
+             }
+             else
+             {
+                 float ttc = 0-(obsDistance/obsSpeed);
+                 double obsDistance_log=0;
+                 if(obsDistance>500)
+                     obsDistance_log=100;
+                 else
+                     obsDistance_log=obsDistance;
+                 QDateTime dt2=QDateTime::currentDateTime();
+                 double disToEndTrace =  GetDistance(now_gps_ins, *gpsMapLine[end_tracepoint]);
+                 double disToParkPoint =  GetDistance(now_gps_ins, aim_gps_ins);
+                 outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
+                         <<"Decide_Dspeed"  << ","  <<setprecision(2)<<dSpeed<< ","
+                         <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
+                         <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
+                         <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
+        //                             <<"readyParkMode"<< ","<<readyParkMode<< ","
+        //                             <<"readyZhucheMode"<< ","<<readyZhucheMode<< ","
+                         <<"trace_disToEndTrace"<< ","<<disToEndTrace<< ","
+                         <<"disToParkPoint"<< ","<<disToParkPoint<< ","
+                         <<"OBS_Distance"<< ","<<obsDistance_log<< ","
+                         <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
+                         <<"obsSpeed_fusion"<<","<<obsSpeed<<","
+                         <<"SecSpeed"<<","<<secSpeed<<","
+                         <<"Vehicle_state"<< ","<<vehState<< ","
+                         <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
+                         <<"avoidXNew"<<","<< setprecision(3)<<avoidXNew<<","
+                         <<"avoidXNewPre"<<","<< setprecision(3)<<avoidXNewPre<<","
+                         <<"Now_s_d_d"<< ","<< setprecision(3)<<now_s_d.d<< ","
+                         <<"Now_s_d_s"<< ","<< setprecision(3)<<now_s_d.s<< ","
+                         //<<"avoidXPre"<<","<<avoidXPre<<","
+                         <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
+                         <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
+                         <<"Min_Decelation"","<<minDecelerate<< ","
+                         <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
+                         <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
+                         <<"Vehicle_GPS_lat"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
+                         <<"Vehicle_GPS_lng"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
+                         <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_x<< ","
+                         <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_y<< ","
+
+                        <<"aim_GPS_lat"<< ","<< setprecision(10)<<aim_gps_ins.gps_lat<< ","
+                        <<"aim_GPS_lng"<< ","<< setprecision(10)<<aim_gps_ins.gps_lng<< ","
+                        <<"aim_GPS_X"<< ","<< setprecision(10)<<aim_gps_ins.gps_x<< ","
+                        <<"aim_GPS_Y"<< ","<< setprecision(10)<<aim_gps_ins.gps_y<< ","
+
+                         <<"MAP_GPS_heading"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->ins_heading_angle<< ","
+                         <<"MAP_GPS_X"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_x<< ","
+                         <<"MAP_GPS_Y"<< ","<< setprecision(10)<<gpsMapLine[PathPoint]->gps_y<< ","
+                         <<"Trace_Point"<< ","<<PathPoint<< ","
+                         <<"changingDangWei"<< ","<<changingDangWei<< ","
+                         <<"gpsTraceOri"<< ","<<gpsTraceOri.size()<< ","
+                         <<"TTC"<< ","<<ttc<< ","
+                         <<"LightColor"  << ","  <<ServiceCarStatus.iTrafficeLightColor<< ","
+                         <<"LightTime"<< ","  <<ServiceCarStatus.iTrafficeLightTime<< ","
+                         <<"DisTostopline"<< ","  <<setprecision(2)<<distance_to_stopline<< ","
+        //                               <<"OBS_Distance"<< ","<<obsDistance_log<< ","
+        //                               <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
+        //                               <<"Vehicle_state"<< ","<<vehState<< ","
+        //                               <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
+        //                               <<"avoidXNew"<<","<<avoidXNew<<","
+        //                               <<"avoidXNewPre"<<","<<avoidXNewPre<<","
+        ////                               <<"avoidXPre"<<","<<avoidXPre<<","
+        //                               <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
+                       <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
+        //                               <<"Min_Decelation"","<<minDecelerate<< ","
+                         <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
+                        // <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
+                        // <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
+                        // <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
+                        // <<"Trace_Point"<< ","<<PathPoint<< ","
+                        // <<"OBS_Speed"<< ","<<obsSpeed<< ","
+                        // <<"OBS_Distance"<< ","<<obsDistance<< ","
+                        // <<"TTC"<< ","<<ttc<< ","
+                         <<endl;
+                 outfile.close();
+             }
+          }
+     }
+
+    lastAngle=gps_decition->wheel_angle;
+    lastVehState=vehState;
+
+    //   qDebug("decide1 time is %d",xTime.elapsed());
+    return gps_decition;
+}
+
+
+
+void iv::decition::DecideGps00::initMethods(){
+
+    pid_Controller= new PIDController();
+
+    ge3_Adapter = new Ge3Adapter();
+    qingyuan_Adapter = new QingYuanAdapter();
+    vv7_Adapter = new VV7Adapter();
+    zhongche_Adapter = new ZhongcheAdapter();
+    bus_Adapter = new BusAdapter();
+    hapo_Adapter = new HapoAdapter();
+    sightseeing_Adapter = new SightseeingAdapter();
+    changanshenlan_Adapter = new ChanganShenlanAdapter();
+
+
+    mpid_Controller.reset(pid_Controller);
+    mge3_Adapter.reset(ge3_Adapter);
+    mqingyuan_Adapter.reset(qingyuan_Adapter);
+    mvv7_Adapter.reset(vv7_Adapter);
+    mzhongche_Adapter.reset(zhongche_Adapter);
+    mbus_Adapter.reset(bus_Adapter);
+    mhapo_Adapter.reset(hapo_Adapter);
+    msightseeing_Adapter.reset(sightseeing_Adapter);
+    mchanganshenlan_Adapter.reset(changanshenlan_Adapter);
+
+    frenetPlanner = new FrenetPlanner();
+    //    laneChangePlanner = new LaneChangePlanner();
+
+    mfrenetPlanner.reset(frenetPlanner);
+    //    mlaneChangePlanner.reset(laneChangePlanner);
+
+}
+
+
+void iv::decition::DecideGps00::phaseSpeedDecition(iv::decition::Decition  decition, double secSpeed, double obsDis, double obsSpeed, GPS_INS gpsIns ) {
+
+    pid_Controller->getControlDecition( gpsIns, gpsTraceNow,  dSpeed,  obsDis,  obsSpeed, false, true, &decition);
+
+    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+    {
+        if((decition->accelerator > minDecelerate) && (minDecelerate < 0))
+        {
+            decition->accelerator = minDecelerate;
+        }
+    }
+
+    if(ServiceCarStatus.msysparam.mvehtype=="ge3"){
+        ge3_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if( ServiceCarStatus.msysparam.mvehtype=="qingyuan"){
+        qingyuan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if( ServiceCarStatus.msysparam.mvehtype=="vv7"){
+        vv7_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if( ServiceCarStatus.msysparam.mvehtype=="zhongche"){
+        zhongche_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if(ServiceCarStatus.msysparam.mvehtype=="bus"){
+        bus_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if(ServiceCarStatus.msysparam.mvehtype=="hapo"){
+        hapo_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }else if(ServiceCarStatus.msysparam.mvehtype=="sightseeing"){
+        sightseeing_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }
+    else if(ServiceCarStatus.msysparam.mvehtype=="shenlan"){
+        changanshenlan_Adapter->getAdapterDeciton(gpsIns, gpsTraceNow, dSpeed, obsDis,obsSpeed,  decition->accelerator,ServiceCarStatus.mfCalAcc  ,changingDangWei, &decition);
+    }
+}
+
+
+
+std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTrace(iv::GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine, int lastIndex, bool circleMode) {
+    vector<iv::Point2D> trace;
+
+    traceOriLeft.clear();
+    traceOriRight.clear();
+    int tracesize=800;
+    if(ServiceCarStatus.msysparam.mvehtype=="shenlan")
+    {
+        tracesize=400;//400;
+    }
+    else
+    {
+        tracesize=800;
+    }
+
+    if (gpsMapLine.size() > tracesize && PathPoint >= 0)
+    {
+        int aimIndex;
+        if(circleMode){
+            aimIndex=PathPoint+tracesize;
+        }else{
+            aimIndex=min((PathPoint+tracesize),(int)gpsMapLine.size());
+        }
+
+        for (int i = PathPoint; i < aimIndex; i++)
+        {
+            int index = i % gpsMapLine.size();
+            Point2D pt = Coordinate_Transfer((*gpsMapLine[index]).gps_x, (*gpsMapLine[index]).gps_y, now_gps_ins);
+            pt.x += offset_real * 0.032;
+            pt.v1 = (*gpsMapLine[index]).speed_mode;
+            pt.v2 = (*gpsMapLine[index]).mode2;
+            pt.roadMode=(*gpsMapLine[index]).roadMode;
+            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
+            {
+                readyZhucheMode = true;
+                DecideGps00::zhuchePointNum = index;
+            }
+
+            if (gpsMapLine[index]->roadMode==3 && !hasZhuched)
+            {
+                readyZhucheMode = true;
+                DecideGps00::zhuchePointNum = index;
+            }
+
+            //csvv7
+            if (gpsMapLine[index]->speed_mode==22 || gpsMapLine[index]->speed_mode==23)
+            {
+                readyParkMode = true;
+                DecideGps00::finishPointNum = index;
+            }
+            switch (pt.v1)
+            {
+            case 0:
+                pt.speed = 80;
+                break;
+            case 1:
+                pt.speed = 25;
+                break;
+            case 2:
+                pt.speed =25;
+                break;
+            case 3:
+                pt.speed = 20;
+                break;
+            case 4:
+                pt.speed =18;
+                break;
+            case 5:
+                pt.speed = 18;
+                break;
+            case 7:
+                pt.speed = 10;
+                break;
+            case 22:
+                pt.speed = 5;
+                break;
+            case 23:
+                pt.speed = 5;
+                break;
+            default:
+                break;
+            }
+            trace.push_back(pt);
+
+
+            double hdg=90 - (*gpsMapLine[index]).ins_heading_angle;
+            if(hdg < 0)  hdg = hdg + 360;
+            if(hdg >= 360) hdg = hdg - 360;
+
+            double xyhdg  = hdg/180.0*PI;
+            double ptLeft_x=(*gpsMapLine[index]).gps_x + ServiceCarStatus.msysparam.vehWidth / 2 * cos(xyhdg + PI / 2);
+            double ptLeft_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg + PI / 2);
+            Point2D ptLeft = Coordinate_Transfer(ptLeft_x, ptLeft_y, now_gps_ins);
+
+            double ptRight_x=(*gpsMapLine[index]).gps_x + ServiceCarStatus.msysparam.vehWidth / 2 * cos(xyhdg - PI / 2);
+            double ptRight_y=(*gpsMapLine[index]).gps_y + ServiceCarStatus.msysparam.vehWidth / 2 * sin(xyhdg - PI / 2);
+            Point2D ptRight = Coordinate_Transfer(ptRight_x, ptRight_y, now_gps_ins);
+
+            traceOriLeft.push_back(ptLeft);
+            traceOriRight.push_back(ptRight);
+
+        }
+    }
+    return trace;
+}
+
+
+std::vector<iv::Point2D> iv::decition::DecideGps00::getGpsTraceAvoid(iv::GPS_INS now_gps_ins, const std::vector<Point2D> gpsTrace, int lastIndex, bool circleMode) {
+    vector<iv::Point2D> trace;
+
+    int index = -1;
+    float minDis = 10;
+
+    for (unsigned int i = 0; i < gpsTrace.size(); i++)
+    {
+        double tmpdis = sqrt((now_gps_ins.gps_x - gpsTrace[i].x) * (now_gps_ins.gps_x - gpsTrace[i].x) + (now_gps_ins.gps_y - gpsTrace[i].y) * (now_gps_ins.gps_y - gpsTrace[i].y));
+
+        if (tmpdis < minDis)
+        {
+            index = i;
+            minDis = tmpdis;
+        }
+    }
+
+    trace.clear();
+    if (index >= 0) {
+        for (unsigned int i = index; i < gpsTrace.size(); i++)
+        {
+            Point2D pt = Coordinate_Transfer(gpsTrace[i].x, gpsTrace[i].y, now_gps_ins);
+            trace.push_back(pt);
+        }
+    }
+    return trace;
+}
+
+
+
+std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffset(std::vector<Point2D> gpsTrace, double offset) {
+
+    if (offset==0)
+    {
+        return gpsTrace;
+    }
+
+    std::vector<iv::Point2D> trace;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        double avoidLenth = abs(offset);
+        if (offset<0)
+        {
+            Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
+                           carFronty + avoidLenth  * sin(anglevalue + PI / 2));
+            ptLeft.speed = gpsTrace[j].speed;
+            ptLeft.v1 = gpsTrace[j].v1;
+            ptLeft.v2 = gpsTrace[j].v2;
+            trace.push_back(ptLeft);
+        }
+        else
+        {
+            Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
+                            carFronty + avoidLenth  * sin(anglevalue - PI / 2));
+            ptRight.speed = gpsTrace[j].speed;
+            ptRight.v1 = gpsTrace[j].v1;
+            ptRight.v2 = gpsTrace[j].v2;
+
+
+            trace.push_back(ptRight);
+        }
+    }
+
+    return trace;
+}
+
+void  iv::decition::DecideGps00::getGpsTraceNowLeftRight(std::vector<Point2D> gpsTrace) {
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (unsigned int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        Point2D ptLeft(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue + PI / 2),
+                       carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue + PI / 2));
+
+        Point2D ptRight(carFrontx + ServiceCarStatus.msysparam.vehWidth / 2 * cos(anglevalue - PI / 2),
+                        carFronty + ServiceCarStatus.msysparam.vehWidth / 2 * sin(anglevalue - PI / 2));
+
+        gpsTraceNowLeft.push_back(ptLeft);
+        gpsTraceNowRight.push_back(ptRight);
+    }
+
+}
+
+
+
+
+std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetAvoid(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps) {
+
+    if (offset==0)
+    {
+        return gpsTrace;
+    }
+
+    std::vector<iv::Point2D> trace;
+    std::vector<iv::Point2D> traceXY;
+    for (int j = 0; j < gpsTrace.size(); j++)
+    {
+        double sumx1 = 0, sumy1 = 0, count1 = 0;
+        double sumx2 = 0, sumy2 = 0, count2 = 0;
+        for (int k = max(0, j - 4); k <= j; k++)
+        {
+            count1 = count1 + 1;
+            sumx1 += gpsTrace[k].x;
+            sumy1 += gpsTrace[k].y;
+        }
+        for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+        {
+            count2 = count2 + 1;
+            sumx2 += gpsTrace[k].x;
+            sumy2 += gpsTrace[k].y;
+        }
+        sumx1 /= count1; sumy1 /= count1;
+        sumx2 /= count2; sumy2 /= count2;
+
+        double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+        double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+        double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+        double avoidLenth = abs(offset);
+        if (offset<0)
+        {
+            Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
+                           carFronty + avoidLenth  * sin(anglevalue + PI / 2));
+            ptLeft.speed = gpsTrace[j].speed;
+            ptLeft.v1 = gpsTrace[j].v1;
+            ptLeft.v2 = gpsTrace[j].v2;
+            trace.push_back(ptLeft);
+        }
+        else
+        {
+            Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
+                            carFronty + avoidLenth  * sin(anglevalue - PI / 2));
+            ptRight.speed = gpsTrace[j].speed;
+            ptRight.v1 = gpsTrace[j].v1;
+            ptRight.v2 = gpsTrace[j].v2;
+
+
+            trace.push_back(ptRight);
+        }
+    }
+
+
+    bool use_new_method = true;
+    if  (use_new_method)
+    {
+        const int val = 300;
+        if(trace.size()>val)
+        {
+            double V = trace[300].y;
+            for (int j = 0; j < val; j++)
+            {
+                double t = (double)j / val;
+                double s = t*t*(3.-2.*t);
+                double ox = s;
+                double oy = t *( V-3.0)+3.0;
+
+                trace[j].x=ox*trace[j].x;
+                trace[j].y=oy;
+            }
+        }
+    }
+
+    traceXY.clear();
+    for(int j=0;j<30;j++)
+    {
+        GPS_INS gpsxy = Coordinate_UnTransfer(gpsTrace[j].x,gpsTrace[j].y,nowgps);
+        Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
+        traceXY.push_back(gpsinxy);
+    }
+    for(int j=0;j<trace.size();j++)
+    {
+        GPS_INS gpsxy = Coordinate_UnTransfer(trace[j].x,trace[j].y,nowgps);
+        Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
+        traceXY.push_back(gpsinxy);
+    }
+    return traceXY;
+}
+
+std::vector<iv::Point2D>  iv::decition::DecideGps00::getGpsTraceOffsetBackOri(std::vector<Point2D> gpsTrace, double offset,GPS_INS nowgps,std::vector<Point2D> gpsTraceNowNow) {
+
+
+    std::vector<iv::Point2D> trace;
+    std::vector<iv::Point2D> traceXY;
+
+    if (offset==0)
+    {
+        trace.assign(gpsTrace.begin(), gpsTrace.end());
+    }
+    else
+    {
+        for (int j = 0; j < gpsTrace.size(); j++)
+        {
+            double sumx1 = 0, sumy1 = 0, count1 = 0;
+            double sumx2 = 0, sumy2 = 0, count2 = 0;
+            for (int k = max(0, j - 4); k <= j; k++)
+            {
+                count1 = count1 + 1;
+                sumx1 += gpsTrace[k].x;
+                sumy1 += gpsTrace[k].y;
+            }
+            for (int k = j; k <= min(gpsTrace.size() - 1, (size_t)(j + 4)); k++)
+            {
+                count2 = count2 + 1;
+                sumx2 += gpsTrace[k].x;
+                sumy2 += gpsTrace[k].y;
+            }
+            sumx1 /= count1; sumy1 /= count1;
+            sumx2 /= count2; sumy2 /= count2;
+
+            double anglevalue = atan2(sumy2 - sumy1, sumx2 - sumx1);
+
+            double carFrontx = gpsTrace[j].x;// -Form1.CarRear * Math.Cos(anglevalue);
+            double carFronty = gpsTrace[j].y;// -Form1.CarRear * Math.Sin(anglevalue);
+
+            double avoidLenth = abs(offset);
+            if (offset<0)
+            {
+                Point2D ptLeft(carFrontx + avoidLenth * cos(anglevalue + PI / 2),
+                               carFronty + avoidLenth  * sin(anglevalue + PI / 2));
+                ptLeft.speed = gpsTrace[j].speed;
+                ptLeft.v1 = gpsTrace[j].v1;
+                ptLeft.v2 = gpsTrace[j].v2;
+                trace.push_back(ptLeft);
+            }
+            else
+            {
+                Point2D ptRight(carFrontx + avoidLenth  * cos(anglevalue - PI / 2),
+                                carFronty + avoidLenth  * sin(anglevalue - PI / 2));
+                ptRight.speed = gpsTrace[j].speed;
+                ptRight.v1 = gpsTrace[j].v1;
+                ptRight.v2 = gpsTrace[j].v2;
+
+
+                trace.push_back(ptRight);
+            }
+        }
+    }
+
+
+    bool use_new_method = true;
+    if  (use_new_method)
+    {
+        const int val = 300;
+        if(trace.size()>val)
+        {
+            double V = trace[300].y;
+            for (int j = 0; j < val; j++)
+            {
+                double t = (double)j / val;
+                double s = t*t*(3.-2.*t);
+                double ox = s;
+                double oy = t *( V-3.0)+3.0;
+
+                trace[j].x=ox*trace[j].x;
+                trace[j].y=oy;
+            }
+        }
+    }
+
+    traceXY.clear();
+    for(int j=0;j<30;j++)
+    {
+        GPS_INS gpsxy = Coordinate_UnTransfer(gpsTraceNowNow[j].x,gpsTraceNowNow[j].y,nowgps);
+        Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
+        traceXY.push_back(gpsinxy);
+    }
+    for(int j=0;j<trace.size();j++)
+    {
+        GPS_INS gpsxy = Coordinate_UnTransfer(trace[j].x,trace[j].y,nowgps);
+        Point2D gpsinxy(gpsxy.gps_x,gpsxy.gps_y);
+        traceXY.push_back(gpsinxy);
+    }
+    return traceXY;
+}
+
+
+
+double iv::decition::DecideGps00::getAngle(std::vector<Point2D> gpsTrace,GPS_INS gpsIns,iv::decition::Decition decition) {
+    double angle=0;
+    if ( abs(iv::decition::DecideGps00().minDis) < 20 && abs(iv::decition::DecideGps00().maxAngle) < 100)
+    {
+        //   angle = iv::decition::Compute00().getDecideAngle(gpsTrace, realspeed);
+        pid_Controller->getControlDecition(gpsIns, gpsTrace,  -1,  -1,  -1, true, false, &decition);
+        angle= decition->wheel_angle;
+    }
+    return angle;
+}
+
+double iv::decition::DecideGps00::getSpeed(std::vector<Point2D> gpsTrace) {
+    double speed=0;
+    int speedPoint = iv::decition::Compute00().getSpeedPointIndex(gpsTrace, max(10.0, realspeed));
+    speed = gpsTrace[speedPoint].speed;
+    for (int i = 0; i < speedPoint; i++) {
+        speed = min(speed, gpsTrace[i].speed);
+    }
+    return speed;
+}
+
+
+//void iv::decition::DecideGps00::getEsrObs(std::vector<iv::ObstacleBasic> obsRadars) {
+//
+//	if (!obsRadars.empty())
+//	{
+//		esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, obsRadars);
+//
+//		if (esrIndex != -1)
+//		{
+//			 esrDistance = obsRadars[esrIndex].nomal_y;
+//
+//
+//
+//			obsSpeed = obsRadars[esrIndex].speed_y;
+//
+//		}
+//		else {
+//			esrDistance = -1;
+//		}
+//
+//	}
+//	else
+//	{
+//		esrIndex = -1;
+//		esrDistance = -1;
+//	}
+//	if (esrDistance < 0) {
+//		ODS("\n------------------>ESR障碍物距离:%f\n", esrDistance);
+//	}
+//	else {
+//		ODS("\n------------------>ESR障碍物距离:%f   ( %.05f , %.05f )\n", esrDistance, obsRadars[esrIndex].nomal_x, obsRadars[esrIndex].nomal_y);
+//	}
+//
+//	ODS("\n------------------>ESR障碍物速度:%f\n", obsSpeed);
+//}
+
+
+
+void iv::decition::DecideGps00::getEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum) {
+
+
+    int esrPathpoint;
+
+    esrIndex = iv::decition::Compute00().getEsrIndex(gpsTrace, roadNum, &esrPathpoint);
+
+    if (esrIndex != -1)
+    {
+        //优化
+        //        double distance = 0.0;
+        //        for(int i=0; i<esrPathpoint; ++i){
+        //            distance+=DirectDistance(gpsTrace[i].x,gpsTrace[i].y,gpsTrace[i+1].x,gpsTrace[i+1].y);
+        //        }
+        //        esrDistance = distance - Esr_Y_Offset;
+        //        if(esrDistance<=0){
+        //            esrDistance=1;
+        //        }
+
+
+        esrDistance = ServiceCarStatus.obs_radar[esrIndex].nomal_y;
+        obsSpeed = ServiceCarStatus.obs_radar[esrIndex].speed_y;
+
+    }
+    else {
+        esrDistance = -1;
+    }
+}
+
+
+
+
+void iv::decition::DecideGps00::getEsrObsDistanceAvoid() {
+
+    esrIndexAvoid = iv::decition::Compute00().getEsrIndexAvoid(gpsTraceAvoid);
+
+    if (esrIndexAvoid != -1)
+    {
+        esrDistanceAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y;
+        obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
+
+    }
+    else {
+        esrDistanceAvoid = -1;
+    }
+
+    if (esrDistanceAvoid < 0) {
+        std::cout << "\nESR障碍物距离Avoid:%f\n" << esrDistanceAvoid << std::endl;
+    }
+    else {
+        std::cout << "\nESR障碍物距离Avoid:%f   %d:( %.05f , %.05f ,%.05f )\n" << esrDistanceAvoid << esrIndexAvoid << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_x << ServiceCarStatus.obs_radar[esrIndexAvoid].nomal_y << std::endl;
+    }
+
+    std::cout << "\nESR障碍物速度Avoid:%f\n" << obsSpeedAvoid <<std::endl;
+}
+
+
+
+
+double iv::decition::DecideGps00::limitAngle(double speed, double angle) {
+    double preAngle = angle;
+
+    if (speed > 15)
+    {
+        if (preAngle > 350)
+        {
+            preAngle = 350;
+        }
+        if (preAngle < -350)
+        {
+            preAngle = -350;
+        }
+    }
+    if (speed > 22)
+    {
+        if (preAngle > 200)
+        {
+            preAngle = 200;
+        }
+        if (preAngle < -200)
+        {
+            preAngle = -200;
+        }
+    }
+    if (speed > 25)
+    {
+        if (preAngle > 150)
+        {
+            preAngle = 150;
+        }
+        if (preAngle < -150)
+        {
+            preAngle = -150;
+        }
+    }
+    if (speed > 30)
+    {
+        if (preAngle > 70)
+        {
+            preAngle = 70;
+        }
+        if (preAngle < -70)
+        {
+            preAngle = -70;
+        }
+    }
+    if (speed > 45)                     //20
+    {
+        if (preAngle > 15)
+        {
+            preAngle = 15;
+        }
+        if (preAngle < -15)
+        {
+            preAngle = -15;
+        }
+    }
+    return preAngle;
+}
+
+
+
+double iv::decition::DecideGps00::limitSpeed(double angle, double speed)
+{
+    if (abs(angle) > 500 && speed > 8)
+        speed = 8;
+    else if (abs(angle) > 350 && speed > 14)
+        speed = 14;
+    else if (abs(angle) > 200 && speed > 21)
+        speed = 21;
+    else if (abs(angle) > 150 && speed > 24)
+        speed = 24;
+    else if (abs(angle) > 60 && speed > 29)
+        speed = 29;
+    else if (abs(angle) > 20 && speed > 34)
+        speed = 34;
+    return max(0.0, speed);
+}
+
+
+bool iv::decition::DecideGps00::checkAvoidEnable(int roadNum)
+{
+    if ((obsDisVector[roadNum] > 0 && obsDisVector[roadNum] < obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth)
+                                   || (obsDisVector[roadNum] > 0 && obsDisVector[roadNum] < 15))
+    {
+        return false;
+    }
+
+    if (roadNum - roadNow > 1)
+    {
+        for (int i = roadNow + 1; i < roadNum; i++)
+        {
+            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0)
+            {
+                return false;
+            }
+        }
+
+    }
+
+    else if (roadNow - roadNum > 1)
+    {
+        for (int i = roadNow-1; i > roadNum; i--)
+        {
+            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0)
+            {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+
+
+bool iv::decition::DecideGps00::checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum) {
+    //lsn
+    if (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<20)
+    {
+        return false;
+    }
+    if ((obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNum]< obsDisVector[roadNow]) ||
+            (obsDisVector[roadNum]>0 && obsDisVector[roadNum]<50 && obsDisVector[roadNow]<0))
+    {
+        return false;
+    }
+
+
+    if (roadNum - roadNow>1)
+    {
+        for (int i = roadNow + 1; i < roadNum; i++)
+        {
+            if (obsDisVector[i] < 8 + (i-roadNow) * 2 && obsDisVector[i]>0) {
+                return false;
+            }
+        }
+
+    }
+
+    else if (roadNow - roadNum>1)
+    {
+        for (int i = roadNow - 1; i >roadNum; i--)
+        {
+            if (obsDisVector[i] < 8 + (roadNow-i) * 2 && obsDisVector[i]>0) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+
+void iv::decition::DecideGps00::getObsAvoid(iv::LidarGridPtr lidarGridPtr) {
+
+
+
+    if (lidarGridPtr == NULL)
+    {
+        iv::decition::DecideGps00::lidarDistanceAvoid = iv::decition::DecideGps00::lastLidarDisAvoid;
+    }
+    else {
+        obsPointAvoid = Compute00().getLidarObsPointAvoid(gpsTraceAvoid, lidarGridPtr);
+        iv::decition::DecideGps00::lastLidarDisAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+    }
+    std::cout << "\nLidarAvoid距离:%f\n" << iv::decition::DecideGps00::lidarDistanceAvoid << std::endl;
+    getEsrObsDistanceAvoid();
+
+    //lidarDistanceAvoid = -1;   //20200103 apollo_fu
+
+    if (esrDistanceAvoid>0 && iv::decition::DecideGps00::lidarDistanceAvoid > 0)
+    {
+        if (iv::decition::DecideGps00::lidarDistanceAvoid >= esrDistanceAvoid)
+        {
+            obsDistanceAvoid = esrDistanceAvoid;
+            obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
+        }
+        else if (!ServiceCarStatus.obs_radar.empty())
+        {
+            obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+            obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
+            std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
+        }
+        else
+        {
+            obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+            obsSpeedAvoid = obsSpeedAvoid = 0 - secSpeed;;
+            std::cout << "\n毫米波无数据，计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
+        }
+    }
+    else if (esrDistanceAvoid>0)
+    {
+        obsDistanceAvoid = esrDistanceAvoid;
+        obsSpeedAvoid = ServiceCarStatus.obs_radar[esrIndexAvoid].speed_y;
+    }
+    else if (iv::decition::DecideGps00::lidarDistanceAvoid > 0)
+    {
+        obsDistanceAvoid = iv::decition::DecideGps00::lidarDistanceAvoid;
+        obsSpeedAvoid = Compute00().getObsSpeed(obsPointAvoid, secSpeed);
+        std::cout << "\n计算obsSpeed:%f\n" << obsSpeedAvoid << std::endl;
+    }
+    else {
+        obsDistanceAvoid = esrDistanceAvoid;
+        obsSpeedAvoid = 0 - secSpeed;
+    }
+
+    std::cout << "\n最终得出的obsDistanceAvoid:%f\n" << obsDistanceAvoid << std::endl;
+
+
+
+
+
+    if (iv::decition::DecideGps00::obsDistanceAvoid <0 && obsLostTimeAvoid<4)
+    {
+        iv::decition::DecideGps00::obsDistanceAvoid = iv::decition::DecideGps00::lastDistanceAvoid;
+        obsLostTimeAvoid++;
+    }
+    else
+    {
+        obsLostTimeAvoid = 0;
+        iv::decition::DecideGps00::lastDistanceAvoid = -1;
+    }
+
+
+
+
+    if (obsDistanceAvoid>0)
+    {
+        iv::decition::DecideGps00::lastDistanceAvoid = obsDistanceAvoid;
+    }
+
+    std::cout << "\nODSAvoid距离:%f\n" << iv::decition::DecideGps00::obsDistanceAvoid << std::endl;
+}
+
+void iv::decition::DecideGps00::init() {
+    for (int i = 0; i < roadSum; i++)
+    {
+        lastEsrIdVector.push_back(-1);
+        lastEsrCountVector.push_back(0);
+        GPS_INS gps_ins;
+        gps_ins.gps_x = 0;
+        gps_ins.gps_y = 0;
+        startAvoidGpsInsVector.push_back(gps_ins);
+        avoidMinDistanceVector.push_back(0);
+    }
+}
+
+
+
+#include <QTime>
+
+void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+
+    //    QTime xTime;
+    //   xTime.start();
+    //    qDebug("time 0 is %d ",xTime.elapsed());
+    double obs,obsSd;
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+
+    }
+    else {
+        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
+        }
+        lidarDistance = obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+        //    lidarDistance=-1;
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+    obs = lidarDistance;
+    obsSd= obsPoint.obs_speed_y;
+
+    if(ServiceCarStatus.useLidarPerPredict){
+        double preDis= predictObsOnRoad(lidar_per, gpsTrace, 1.0);
+        if (preDis<obs){
+            obs = preDis;
+            if(abs(obs-preDis>0.5)){
+                obsSd = 0-realspeed;
+            }
+        }
+    }
+
+
+    if(roadNum==roadNow){
+        obsDistance=obs;
+        obsSpeed=obsSd;
+    }
+
+
+    if(obsDistance<500&&obsDistance>0){
+        std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "obsDistance<500:" << obsDistance << std::endl;
+    }
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
+
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    //lsn
+    if(obs<0){
+        obsDisVector[roadNum]=500;
+    }else{
+        obsDisVector[roadNum]=obs;
+    }
+
+    //   qDebug("time 3 is %d ",xTime.elapsed());
+
+}
+
+//void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
+//                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+void iv::decition::DecideGps00::computeObsOnRoadXY( iv::GPS_INS now_gps_ins,iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
+                                                    const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+
+    //    QTime xTime;
+    //   xTime.start();
+    //    qDebug("time 0 is %d ",xTime.elapsed());
+    double obs,obsSd;
+    double obsCarCoordinateX,obsCarCoordinateY;
+    GPS_INS obsGeodetic;
+    Point2D obsFrenet,obsFrenetMid;
+    double obsCarCoordinateDistance=-1;
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+
+    }
+    else {
+        //obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+
+        obsPoint = Compute00().getLidarObsPointXY(gpsTrace,gpsTraceLeft,gpsTraceRight,lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
+        }
+
+        obsCarCoordinateX=obsPoint.x;
+        obsCarCoordinateY=obsPoint.y;
+        obsGeodetic = Coordinate_UnTransfer(obsCarCoordinateX, obsCarCoordinateY, now_gps_ins);   //车体转大地
+        obsFrenetMid=cartesian_to_frenet1D(gpsMapLine,obsGeodetic.gps_x,obsGeodetic.gps_y);          //大地转frenet
+        iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
+        givlog->debug("decition_brain","road_num: %d,obsFrenetMid_s: %f,now_s: %f",roadNum,obsFrenetMid.s,now_s_d.s);
+        obsFrenet.s=obsFrenetMid.s-now_s_d.s;
+        lidarDistance = obsFrenet.s;//obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+        //    lidarDistance=-1;
+//        if(obsPoint.s < 0)
+//        {
+//           lidarDistance = -1;
+//        }
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+
+
+
+
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+    obs = lidarDistance;
+
+    obsSd= obsPoint.obs_speed_y;
+
+    if(ServiceCarStatus.useLidarPerPredict){
+        double preDis= predictObsOnRoad(lidar_per, gpsTrace, 1.0);
+        if (preDis<obs){
+            obs = preDis;
+            if(abs(obs-preDis>0.5)){
+                obsSd = 0-realspeed;
+            }
+        }
+    }
+
+
+    if(roadNum==roadNow){
+        obsDistance=obs;
+        obsSpeed=obsSd;
+    }
+
+    if(obsPoint.obs_type==2){
+        obsSpeed=-secSpeed;
+    }
+
+    if(obsDistance<500&&obsDistance>0){
+        std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "obsDistance<500:" << obsDistance << std::endl;
+    }
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
+
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    //lsn
+    if(obs<0){
+        obsDisVector[roadNum]=500;
+    }else{
+        obsDisVector[roadNum]=obs;
+    }
+
+    //   qDebug("time 3 is %d ",xTime.elapsed());
+
+}
+
+
+//1220
+void iv::decition::DecideGps00::computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+                                                     const std::vector<GPSData> gpsMapLine) {
+
+    double obs,obsSd;
+
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+
+    }
+    else {
+        obsPoint = Compute00().getLidarRearObsPoint(gpsTrace, lidarGridPtr);
+        float lidarXiuZheng=0;
+        if(!ServiceCarStatus.useMobileEye){
+            lidarXiuZheng = ServiceCarStatus.msysparam.rearGpsXiuzheng;
+        }
+        if(abs(obsPoint.y)>lidarXiuZheng)
+            lidarDistance = abs(obsPoint.y)-lidarXiuZheng;   //激光距离推到车头 1220
+
+        //    lidarDistance=-1;
+        if (lidarDistance<0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+
+
+    if(lidarDistance<0){
+        lidarDistance=500;
+    }
+
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "激光雷达距离:" << lidarDistance << std::endl;
+
+    if(lidarDistance==500){
+        lidarDistance=-1;
+    }
+
+
+
+
+    ServiceCarStatus.mLidarObs = lidarDistance;
+
+    obs=lidarDistance;
+    //	obsSpeed = 0 - secSpeed;
+    obsSd = 0 -secSpeed;
+
+
+
+
+    if(roadNum==roadNow){
+        obsDistance=obs;
+        obsSpeed=obsSd;
+    }
+
+
+    if (obsDistance <0 && obsLostTime<4)
+    {
+        obsDistance = lastDistance;
+        obsLostTime++;
+    }
+    else
+    {
+        obsLostTime = 0;
+        lastDistance = -1;
+    }
+
+
+    std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100){
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    //lsn
+    if(obs<0){
+        obsDisVector[roadNum]=500;
+    }else{
+        obsDisVector[roadNum]=obs;
+    }
+
+}
+
+
+
+double iv::decition::DecideGps00::predictObsOnRoad(std::vector<iv::Perception::PerceptionOutput> lidar_per,std::vector<Point2D> gpsTrace,double realSpeed){
+    double preObsDis=500;
+
+    if(!lidar_per.empty()){
+        preObsDis=PredictObsDistance(  realSpeed,  gpsTrace, lidar_per);
+        lastPreObsDistance=preObsDis;
+
+    }else{
+        preObsDis=lastPreObsDistance;
+    }
+
+    ServiceCarStatus.mfttc = preObsDis;
+    return preObsDis;
+    //    if(preObsDis<obsDistance){
+    //        obsDistance=preObsDis;
+    //        lastDistance=obsDistance;
+    //    }
+}
+
+int iv::decition::DecideGps00::chooseAvoidRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+    roadPre = -1;
+
+    for (int i =  0; i < roadSum; i++)
+    {
+        gpsTraceAvoid.clear();
+        avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint]);
+        gpsTraceAvoid = getGpsTraceOffset(gpsTraceOri, avoidX);
+        computeObsOnRoad(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,lidar_per);
+    }
+
+    if (lidarGridPtr!=NULL)
+    {
+        hasCheckedAvoidLidar = true;
+    }
+
+    for(int i=0; i<roadSum;i++){
+        std::cout<<"\odsjuli====================:\n"<<i<<"chedaojuli:"<<obsDisVector[i]<<endl;
+
+    }
+
+    checkAvoidObsTimes++;
+    if (checkAvoidObsTimes<4 || hasCheckedAvoidLidar==false)
+    {
+        return - 1;
+    }
+
+
+    for (int i = 1; i < roadSum; i++)
+    {
+        if (roadNow + i < roadSum) {
+            //   avoidX = (roadOri-roadNow-i)*roadWidth;
+            avoidX=computeAvoidX(roadNow+i,roadOri,gpsMapLine[PathPoint]);
+            if (checkAvoidEnable(roadNow+i))
+            {
+                /*if (roadNow==roadOri)
+                {
+                    avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
+                    startAvoid_gps_ins = now_gps_ins;
+                }	*/
+                avoidMinDistanceVector[roadNow]= obsDisVector[roadNow] + 2*ServiceCarStatus.msysparam.vehLenth;
+                startAvoidGpsInsVector[roadNow] = now_gps_ins;
+                roadPre = roadNow + i;
+                return roadPre;
+            }
+        }
+
+        if (roadNow - i >= 0)
+        {
+            //    avoidX = (roadOri - roadNow+i)*roadWidth;
+            avoidX=computeAvoidX(roadNow-i,roadOri,gpsMapLine[PathPoint]);
+            if (checkAvoidEnable(roadNow - i))
+            {
+                /*if (roadNow == roadOri)
+                {
+                    avoidRunDistance = obsDisVector[roadNow] + ServiceCarStatus.msysparam.vehLenth;
+                    startAvoid_gps_ins = now_gps_ins;
+                }*/
+                avoidMinDistanceVector[roadNow] = obsDisVector[roadNow] + 2*ServiceCarStatus.msysparam.vehLenth;
+                startAvoidGpsInsVector[roadNow] = now_gps_ins;
+                roadPre = roadNow - i;
+                return roadPre;
+            }
+        }
+
+    }
+    return roadPre;
+}
+
+int iv::decition::DecideGps00::chooseBackRoad(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, std::vector<GPSData> gpsMapLine,std::vector<iv::Perception::PerceptionOutput> lidar_per)
+{
+    roadPre = -1;
+
+    for (int i =  0; i <roadSum; i++)
+    {
+        gpsTraceBack.clear();
+        avoidX=computeAvoidX(i,roadOri,gpsMapLine[PathPoint]);
+        gpsTraceBack = getGpsTraceOffset(gpsTraceOri, avoidX);
+        computeObsOnRoad(lidarGridPtr, gpsTraceBack, i,gpsMapLine,lidar_per);
+    }
+
+    if (lidarGridPtr != NULL)
+    {
+        hasCheckedBackLidar = true;
+    }
+
+    checkBackObsTimes++;
+    if (checkBackObsTimes<4 || hasCheckedBackLidar == false)
+    {
+        return -1;
+    }
+
+    if ((GetDistance(now_gps_ins, startAvoidGpsInsVector[roadOri])>max(avoidMinDistanceVector[roadOri],40.0)) &&
+            (checkReturnEnable(avoidX, lidarGridPtr, roadOri)))
+    {
+        roadPre = roadOri;
+        return roadPre;
+    }
+
+    if (roadNow-roadOri>1)
+    {
+        for (int i = roadOri + 1;i < roadNow;i++) {
+            if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>  avoidMinDistanceVector[i])&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])> 30.0))
+            {
+                roadPre = i;
+                return roadPre;
+            }
+        }
+    }
+
+    else if (roadNow <roadOri-1)
+    {
+        for (int i = roadOri - 1;i > roadNow;i--) {
+            if (checkReturnEnable(avoidX, lidarGridPtr, i)&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>avoidMinDistanceVector[i])&&
+                    (GetDistance(now_gps_ins, startAvoidGpsInsVector[i])>  30.0))
+            {
+                roadPre = i;
+                return roadPre;
+            }
+        }
+    }
+
+    return roadPre;
+}
+
+
+double iv::decition::DecideGps00::trumpet() {
+    if (trumpetFirstCount)
+    {
+        trumpetFirstCount = false;
+        trumpetLastTime= GetTickCount();
+        trumpetTimeSpan = 0.0;
+    }
+    else
+    {
+        trumpetStartTime= GetTickCount();
+        trumpetTimeSpan += trumpetStartTime - trumpetLastTime;
+        trumpetLastTime = trumpetStartTime;
+    }
+
+    return trumpetTimeSpan;
+}
+
+
+
+
+double iv::decition::DecideGps00::transferP() {
+    if (transferFirstCount)
+    {
+        transferFirstCount = false;
+        transferLastTime= GetTickCount();
+        transferTimeSpan = 0.0;
+    }
+    else
+    {
+        transferStartTime= GetTickCount();
+        transferTimeSpan += transferStartTime - transferLastTime;
+        transferLastTime = transferStartTime;
+    }
+
+    return transferTimeSpan;
+}
+
+
+
+void  iv::decition::DecideGps00::handBrakePark(iv::decition::Decition  decition, long duringTime, GPS_INS now_gps_ins)
+{
+    if (abs(now_gps_ins.speed)>0.1)
+    {
+        decition->accelerator = 0;
+        decition->brake = 20;
+        decition->wheel_angle = 0;
+
+    }
+    else
+    {
+        decition->accelerator = 0;
+        decition->brake = 20;
+        decition->wheel_angle = 0;
+        handPark = true;
+        handParkTime = duringTime;
+    }
+}
+
+
+
+
+void iv::decition::DecideGps00::getMapBeside(std::vector<iv::GPSData> navigation_data, iv::GPS_INS now_gps_ins)
+{
+    gmapsL.clear();
+    gmapsR.clear();
+    for (int i = 0; i < 31; i++)
+    {
+        std::vector<iv::GPSData> gpsMapLineBeside;
+        //	gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, -0.5*i);
+        gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, -0.5*i);
+        gmapsL.push_back(gpsMapLineBeside);
+    }
+
+    for (int i = 0; i < 31; i++)
+    {
+        std::vector<iv::GPSData> gpsMapLineBeside;
+        //		gpsMapLineBeside = iv::decition::ComputeUnit().getBesideGpsMapLine(*ServiceCarStatus.location, navigation_data, 0.5*i);
+        gpsMapLineBeside = iv::decition::Compute00().getBesideGpsMapLine(now_gps_ins, navigation_data, 0.5*i);
+        gmapsR.push_back(gpsMapLineBeside);
+    }
+}
+
+
+bool iv::decition::DecideGps00::checkChaoCheBack(iv::LidarGridPtr lidarGridPtr)
+{
+    if (lidarGridPtr == NULL)
+    {
+        return false;
+        //	lidarDistance = lastlidarDistance;
+    }
+    else
+    {
+        obsPoint = Compute00().getLidarObsPoint(gpsTraceOri, lidarGridPtr);
+        double lidarDistance = obsPoint.y - 2.5;   //激光距离推到车头
+        //     ODS("\n超车雷达距离:%f\n", lidarDistance);
+        if (lidarDistance >-20 && lidarDistance<35)
+        {
+            checkChaoCheBackCounts = 0;
+            return false;
+        }
+        else
+        {
+            checkChaoCheBackCounts++;
+        }
+        if (checkChaoCheBackCounts>2)
+        {
+            checkChaoCheBackCounts = 0;
+            return true;
+        }
+    }
+    return false;
+}
+
+void iv::decition::DecideGps00::updateGroupDate(GPS_INS now_gps_ins,float realspeed,float theta,float s)
+{
+    Point2D pt = Coordinate_Transfer( now_gps_ins.gps_x, now_gps_ins.gps_y, group_ori_gps);  //大地转车体
+
+    ServiceCarStatus.group_x_local=pt.x;
+    //  ServiceCarStatus.group_y_local=pt.y;
+    ServiceCarStatus.group_y_local=s;
+    if(realspeed < 0.36)
+    {
+        ServiceCarStatus.group_velx_local = 0;
+        ServiceCarStatus.group_vely_local = 0;
+    }
+    else
+    {
+        ServiceCarStatus.group_velx_local = realspeed * sin(theta) / 3.6;
+        ServiceCarStatus.group_vely_local = realspeed * cos(theta) / 3.6;
+    }
+
+    ServiceCarStatus.group_pathpoint = PathPoint;
+}
+
+
+
+float  iv::decition::DecideGps00::ComputeTrafficLightSpeed(int traffic_light_color, const std::vector<GPSData> gpsMapLine,
+                                                           int traffic_light_pathpoint, int pathpoint,float secSpeed)
+{
+    float traffic_speed = 200;
+    float traffic_dis = 0;
+
+    if(abs(secSpeed)<0.1)
+    {
+        secSpeed=0;
+    }
+
+    if(pathpoint <= traffic_light_pathpoint)
+    {
+        for(int i=pathpoint;i<traffic_light_pathpoint;i++)
+        {
+            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }
+    else
+    {
+        for(int i = pathpoint; i < gpsMapLine.size()-1; i++)
+        {
+            traffic_dis += GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+        for(int i = 0; i < traffic_light_pathpoint; i++)
+        {
+            traffic_dis += GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }
+
+    if(traffic_light_color==0 && traffic_dis<10)
+    {
+        traffic_speed=0;
+    }
+
+    return  traffic_speed;
+}
+
+float  iv::decition::DecideGps00:: ComputeTrafficLightSpeed(int traffic_light_color, int traffic_light_time,  const std::vector<GPSData> gpsMapLine,int traffic_light_pathpoint,
+                                                           int pathpoint,float secSpeed,float dSpeed){
+    float traffic_speed=200;
+    float traffic_dis=0;
+    float passTime;
+    float passSpeed;
+    bool passEnable=false;
+
+    if(abs(secSpeed)<0.1){
+        secSpeed=0;
+    }
+
+
+
+    if(pathpoint <= traffic_light_pathpoint){
+        for(int i=pathpoint;i<traffic_light_pathpoint;i++){
+            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }else{
+        for(int i=pathpoint;i<gpsMapLine.size()-1;i++){
+            traffic_dis +=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+        for(int i=0;i<traffic_light_pathpoint;i++){
+            traffic_dis+=GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }
+
+    //    if(traffic_light_color != 0)
+    //    {
+    //        int a = 3;
+    //    }
+
+
+//    if(traffic_light_color==0 && traffic_dis<10){
+//        traffic_speed=0;
+//    }
+
+    if(traffic_light_color==2 && traffic_dis<10){  //cxw,1xhonglvdeng gaibian
+        traffic_speed=0;
+//        return  traffic_speed;
+//        if(traffic_dis>3.0)
+//        {
+//            traffic_speed=sqrt(traffic_dis*2.0*9.8*0.5);
+//        }
+//        else
+//        {
+//           //minDecelerate=-0.7;
+//           traffic_speed=0;
+//        }
+        return  traffic_speed;
+    }
+    //    else   //20200108
+    //    {
+    //        traffic_speed=10;
+    //    }
+//    return  traffic_speed;
+
+    passSpeed = min((float)(dSpeed/3.6),secSpeed);
+    passTime = traffic_dis/(dSpeed/3.6);
+
+
+
+//1+x V2R    //1:绿灯 2：红灯 3：黄灯
+    //最开始0x0: 红色 0x1: 黄色 0x2: 绿色
+
+    switch(traffic_light_color){
+    case 2://case 0: //for 1+x修改
+        if(passTime>traffic_light_time+1 && traffic_dis>10){
+            passEnable=true;
+        }else{
+            passEnable=false;
+        }
+        break;
+   case 3:// case 1:
+        if(passTime<traffic_light_time-1 && traffic_dis<10){
+            passEnable=true;
+        }else{
+            passEnable = false;
+        }
+        break;
+    case 1://case 2:
+        if(passTime<traffic_light_time){
+            passEnable= true;
+        }else{
+            passEnable=false;
+        }
+        break;
+
+
+    default:
+        passEnable=true; //20230413
+        break;
+    }
+
+    if(!passEnable){
+        if(traffic_dis<5){
+            traffic_speed=0;
+        }else if(traffic_dis<10){
+            traffic_speed=5;
+        }else if(traffic_dis<20){
+            traffic_speed=15;
+        }else if(traffic_dis<30){
+            traffic_speed=25;
+        }else if(traffic_dis<50){
+            traffic_speed=30;
+        }
+    }
+    return traffic_speed;
+
+}
+void iv::decition::DecideGps00::computeObsOnRoadByFrenet(iv::LidarGridPtr lidarGridPtr, const std::vector<Point2D>& gpsTrace, double & obs,
+                                                         const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
+{
+    //    Point2D obsCombinePoint = Point2D(-1,-1);
+    iv::decition::FrenetPoint car_now_frenet_point = iv::decition::FrenetPlanner::getFrenetfromXY(0,0,gpsTrace,gpsMap,pathpoint,nowGps);
+    double obsSd;
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+        //	lidarDistance = lastlidarDistance;
+    }
+    else
+    {
+        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+        //        lidarDistance = obsPoint.y-2.5;   //激光距离推到车头
+        iv::decition::FrenetPoint lidarFPointTmp = iv::decition::FrenetPlanner::getFrenetfromXY(this->obsPoint.x,this->obsPoint.y,gpsTrace,gpsMap,pathpoint,nowGps);
+        lidarDistance = lidarFPointTmp.s - car_now_frenet_point.s - 2.5;//激光距离推到车头 此处的参数要不要改改！！！！ 2022 apollo_fu
+
+        //    lidarDistance=-1;
+        if (lidarDistance < 0)
+        {
+            lidarDistance = -1;
+        }
+        lastLidarDis = lidarDistance;
+    }
+
+    FrenetPoint esr_obs_frenet_point;
+    getEsrObsDistanceByFrenet(gpsTrace, car_now_frenet_point,esr_obs_frenet_point,gpsMap,pathpoint,nowGps);
+
+    if(lidarDistance < 0){
+        lidarDistance = 500;
+    }
+    if(esrDistance < 0){
+        esrDistance = 500;
+    }
+
+    std::cout << ServiceCarStatus.mRunTime.elapsed() << "ms:" << "激光雷达距离:" << lidarDistance << std::endl;
+    std::cout << ServiceCarStatus.mRunTime.elapsed() << "ms:" << "毫米波距离:" << esrDistance << std::endl;
+    myesrDistance = esrDistance;
+
+    if(lidarDistance == 500)
+    {
+        lidarDistance = -1;
+    }
+    if(esrDistance == 500)
+    {
+        esrDistance = -1;
+    }
+
+    ServiceCarStatus.mRadarObs = esrDistance;
+    ServiceCarStatus.mLidarObs = lidarDistance;
+
+    if (esrDistance>0 && lidarDistance > 0)
+    {
+        if (lidarDistance >= esrDistance)
+        {
+            obs = esrDistance;
+            //            obsSd=ServiceCarStatus.obs_radar[esrIndex].speed_y;
+            obsSd = obsSpeed;
+            //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
+            //            obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
+        }
+        else if (!ServiceCarStatus.obs_radar.empty())
+        {
+            obs = lidarDistance;
+            //            obsCombinePoint = obsPoint;
+            //            obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace);
+            obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
+            std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
+        }
+        else
+        {
+            obs=lidarDistance;
+            //            obsCombinePoint = obsPoint;
+            obsSd = 0 -secSpeed * cos(car_now_frenet_point.tangent_Ang-PI/2);
+            std::cout << ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "毫米波无数据，计算obsSpeed:" << obsSpeed << std::endl;
+        }
+    }
+    else if (esrDistance>0)
+    {
+        obs = esrDistance;
+        //        obsSd = ServiceCarStatus.obs_radar[esrIndex].speed_y;
+        obsSd = obsSpeed;
+        //障碍物信息使用毫米波采集的。其位置坐标加上偏移量，转换成相对于惯导的位置，即转换成车辆坐标系下的位置坐标。
+        //        obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
+    }
+    else if (lidarDistance > 0)
+    {
+        obs = lidarDistance;
+        //        obsCombinePoint = obsPoint;
+        obsSd = Compute00().getObsSpeedByFrenet(obsPoint, secSpeed,gpsTrace,gpsMap,pathpoint,nowGps);
+        std::cout <<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "计算obsSpeed:" << obsSpeed << std::endl;
+    }
+    else
+    {
+        obs = esrDistance;
+        //        obsCombinePoint = Point2D(ServiceCarStatus.obs_radar[esrIndex].nomal_x+Esr_Offset,ServiceCarStatus.obs_radar[esrIndex].nomal_y+Esr_Y_Offset);
+        obsSd = 0 - secSpeed*cos(car_now_frenet_point.tangent_Ang-PI/2);
+    }
+
+    obsDistance = obs;
+    obsSpeed = obsSd;
+
+    std::cout << ServiceCarStatus.mRunTime.elapsed()<<"ms:"<< "最终得出的obsDistance:" << obsDistance << std::endl;
+
+    ServiceCarStatus.mObs = obsDistance;
+    if(ServiceCarStatus.mObs>100)
+    {
+        ServiceCarStatus.mObs =-1;
+    }
+
+    if (obsDistance>0)
+    {
+        lastDistance = obsDistance;
+    }
+
+    if(obs<0)
+    {
+        obsDistance=500;
+    }
+    else
+    {
+        obsDistance=obs;
+    }
+}
+
+void iv::decition::DecideGps00::getRearEsrObsDistance(std::vector<Point2D> gpsTrace, int roadNum)
+{
+    esrIndex = iv::decition::Compute00().getRearEsrIndex(gpsTrace, roadNum);
+
+    if (esrIndex != -1)
+    {
+        esrDistance = ServiceCarStatus.obs_rear_radar[esrIndex].nomal_y;
+        obsSpeed = ServiceCarStatus.obs_rear_radar[esrIndex].speed_y;
+    }
+    else
+    {
+        esrDistance = -1;
+    }
+}
+
+void iv::decition::DecideGps00::getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, const FrenetPoint car_now_frenet_point,
+                                                          FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps)
+{
+    esrIndex = iv::decition::Compute00().getEsrIndexByFrenet(gpsTrace,esrObs_F_Point,gpsMap,pathpoint,nowGps);
+    if (esrIndex != -1)
+    {
+        //障碍物的距离，是障碍物和车辆沿道路上的距离，即s。并不直接是它的x坐标或y坐标或两点直线坐标。
+        //严格来说应是 esrDistance = 障碍物的s - 车辆的s - Esr_Y_Offset，但这是根据车辆位置实时搜索路径，故车辆的s极其小。
+        esrDistance = esrObs_F_Point.s - car_now_frenet_point.s - Esr_Y_Offset;  //-Esr_Y_Offset是把距离推到车头，换算frenet坐标时，把它加上了的。故需减去。
+
+        double speedx = ServiceCarStatus.obs_radar[esrIndex].speed_x;  //障碍物相对于车辆x轴的速度
+        double speedy = ServiceCarStatus.obs_radar[esrIndex].speed_y;  //障碍物相对于车辆y轴的速度
+        double speed_combine = sqrt(speedx * speedx + speedy * speedy);    //将x、y轴两个方向的速度求矢量和
+        //障碍物的速度方向与道路方向的夹角。用于将速度分解到s方向和d方向。
+        //所谓道路方向是指，道路上离障碍物的最近的点沿道路弧线的切线方向。
+        double Etheta = esrObs_F_Point.tangent_Ang - atan2(speedy,speedx);
+
+        obsSpeed = speed_combine * cos(Etheta);  //由speed_combine分解的s轴方向上的速度
+    }
+    else
+    {
+        esrDistance = -1;
+    }
+}
+
+void iv::decition::DecideGps00::getV2XTrafficPositionVector(const std::vector<GPSData> gpsMapLine)
+{
+    v2xTrafficVector.clear();
+    for (int var = 0; var < gpsMapLine.size(); var++)
+    {
+        if(gpsMapLine[var]->roadMode == 6 || gpsMapLine[var]->mode2 == 1000001)
+        {
+            v2xTrafficVector.push_back(var);
+        }
+    }
+}
+
+float iv::decition::DecideGps00::ComputeV2XTrafficLightSpeed(iv::TrafficLight trafficLight, const std::vector<GPSData> gpsMapLine,std::vector<int> v2xTrafficVector,
+                                                             int pathpoint,float secSpeed,float dSpeed, bool circleMode)
+{
+    float trafficSpeed = 200;
+    int nearTraffixPoint = -1;
+    float nearTrafficDis = 0;
+    int traffic_color = 0;
+    int traffic_time = 0;
+    bool passThrough = false;
+    float dSecSpeed = dSpeed / 3.6;
+
+    if(v2xTrafficVector.empty())
+    {
+        return trafficSpeed;
+    }
+    if(!circleMode)
+    {
+        if(pathpoint > v2xTrafficVector.back())
+        {
+            return trafficSpeed;
+        }
+        else
+        {
+            for(int i = 0; i < v2xTrafficVector.size();i++)
+            {
+                if (pathpoint <= v2xTrafficVector[i])
+                {
+                    nearTraffixPoint = v2xTrafficVector[i];
+                    break;
+                }
+            }
+        }
+    }
+    else if(circleMode)
+    {
+        if(pathpoint > v2xTrafficVector.back())
+        {
+            nearTraffixPoint = v2xTrafficVector[0];
+        }
+        else
+        {
+            for(int i=0; i < v2xTrafficVector.size();i++)
+            {
+                if (pathpoint <= v2xTrafficVector[i])
+                {
+                    nearTraffixPoint = v2xTrafficVector[i];
+                    break;
+                }
+            }
+        }
+    }
+
+    if(nearTraffixPoint != -1)
+    {
+        for(int i = pathpoint;i < nearTraffixPoint;i++)
+        {
+            nearTrafficDis += GetDistance(*gpsMapLine[i],*gpsMapLine[i+1]);
+        }
+    }
+
+    if(nearTrafficDis>50)
+    {
+        return trafficSpeed;
+    }
+
+    int roadMode = gpsMapLine[pathpoint]->roadMode;
+    if(roadMode == 14 || roadMode == 16)
+    {
+        traffic_color = trafficLight.leftColor;
+        traffic_time = trafficLight.leftTime;
+    }
+    else if(roadMode == 15 ||roadMode == 17)
+    {
+        traffic_color = trafficLight.rightColor;
+        traffic_time = trafficLight.rightTime;
+    }
+    else
+    {
+        traffic_color = trafficLight.straightColor;
+        traffic_time = trafficLight.straightTime;
+    }
+
+//    passThrough = computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,dSecSpeed);
+//    if(passThrough)
+//    {
+//        return trafficSpeed;
+//    }
+//    else
+//    {
+//        trafficSpeed = computeTrafficSpeedLimt(nearTrafficDis);
+//        if(nearTrafficDis < 6)
+//        {
+//            float decelerate = 0 - ( secSpeed * secSpeed * 0.5 / nearTrafficDis);
+//            minDecelerate = min(minDecelerate,decelerate);
+//        }
+//        return trafficSpeed;
+//    }
+    double fTrafficBrake = 1.0;  //移植吉利项目的v2x
+    passThrough=computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,secSpeed,dSecSpeed);
+    if(passThrough){
+        return trafficSpeed;
+    }else{
+        trafficSpeed=computeTrafficSpeedLimt(nearTrafficDis);
+        if(nearTrafficDis<(dSecSpeed*dSecSpeed/(2*fTrafficBrake))){
+            if(fabs(nearTrafficDis)<1.0)
+            {
+                minDecelerate = -1.0 * fTrafficBrake;
+            }
+            else
+            {
+                if((0.5*secSpeed*secSpeed/fTrafficBrake) > (nearTrafficDis*0.9))
+                {
+                    float decelerate =0-( secSpeed*secSpeed*0.5/(nearTrafficDis-0.5));
+                    minDecelerate=min(minDecelerate,decelerate);
+                }
+            }
+        }
+        return trafficSpeed;
+    }
+
+    return trafficSpeed;
+}
+
+//bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float dSecSpeed)
+bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed)
+{
+    float passTime=0;
+    double fAcc = 0.6;
+    if(dSecSpeed == 0)return true;
+    if (trafficColor == 2 || trafficColor == 3)
+    {
+        return false;
+    }
+    else if(trafficColor==0)
+    {
+        return true;
+    }
+    else
+    {
+        passTime=trafficDis/dSecSpeed;
+        if(passTime+1< trafficTime)
+        {
+            double passTime2= trafficTime - 0.1;
+            double disthru = realSecSpeed * passTime2 + 0.5 * fAcc * pow(passTime2,2);
+            if((disthru -1.0)< trafficDis )
+            {
+                if((realSecSpeed<3.0)&&(trafficDis>10))
+                {
+                    return true;
+                }
+                return false;
+            }
+            return true;
+        }
+        else
+        {
+            return false;
+        }
+    }
+}
+
+float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis)
+{
+    float limit=200;
+    double fBrake = 1.0;
+    if(trafficDis < 1.0)
+    {
+        limit =0;
+    }
+    else
+    {
+        limit = 3.6*sqrt(2.0* fabs(fBrake) * (trafficDis-1.0) );
+    }
+    return limit;
+    if(trafficDis<10)
+    {
+        limit = 0;
+    }
+    else if(trafficDis<15)
+    {
+        limit = 5;
+    }
+    else if(trafficDis<20)
+    {
+        limit=10;
+    }
+    else if(trafficDis<30)
+    {
+        limit=15;
+    }
+    return limit;
+}
+
+void iv::decition::DecideGps00::transferGpsMode2( const std::vector<GPSData> gpsMapLine)
+{
+    static int obstacle_disable = 0;
+    static int speed_slowdown_flag = 0;
+    static bool lock_flag = false;
+    double forecast_distance = 0;
+    int forecast_point_num = 0;
+    bool cross = false;
+
+    double secLowSpeed = ServiceCarStatus.mroadmode_vel.mfmode18 / 3.6;   //m/s
+    if(secSpeed > secLowSpeed)
+    {
+        forecast_distance = secSpeed * secSpeed - secLowSpeed * secLowSpeed + 5;
+        forecast_point_num = ((int)forecast_distance) * 10;
+        if((PathPoint + forecast_point_num + 2) > gpsMapLine.size())
+            forecast_point_num = 0;
+    }
+    if((PathPoint + forecast_point_num-8 > 0) && (PathPoint + forecast_point_num + 8 < gpsMapLine.size()))
+    {
+        for(int i = PathPoint + forecast_point_num - 8; i < PathPoint + forecast_point_num + 8; i++)
+        {
+            if(gpsMapLine[i]->mode2 == 5000)
+                cross = true;
+        }
+    }
+
+    //givlog->debug("decition_brain","PATHFORE: %d,Forecast: %d,cross: %d", PathPoint + forecast_point_num,forecast_point_num,cross);
+    if(gpsMapLine[PathPoint]->mode2 == 3000)
+    {
+        if(obsDistance>4)
+        {       //7   zj-5
+            obsDistance = 200;
+        }
+        else
+        {
+            lock_flag = false;
+            obsSpeed  = -realspeed / 3.6;
+        }
+
+        if((realspeed > 3) && (lock_flag == false))
+        {
+            minDecelerate = -0.5;
+        }
+        else
+        {
+            dSpeed = min(dSpeed,3.0);
+            lock_flag = true;
+        }
+    }
+    else if(gpsMapLine[PathPoint]->mode2 == 3001)
+    {
+        obstacle_disable = 1;
+    }
+    else if(gpsMapLine[PathPoint]->mode2 == 3002)
+    {
+        obstacle_disable = 0;
+    }
+    else if(gpsMapLine[PathPoint]->mode2 == 4000)
+    {
+        //ServiceCarStatus.msysparam.vehWidth=5.6;
+    }
+    else if(cross == true)
+    {
+        speed_slowdown_flag =1;
+        lock_flag = false;
+    }
+    else if(gpsMapLine[PathPoint]->mode2==5001)
+    {
+        speed_slowdown_flag=0;
+    }
+
+    if(obstacle_disable==1)
+    {
+        obsDistance=200;
+    }
+
+    if(speed_slowdown_flag == 1)
+    {
+        if((realspeed > ServiceCarStatus.mroadmode_vel.mfmode18) && (lock_flag == false))
+        {
+            minDecelerate = -0.3;
+        }
+        else
+        {
+            dSpeed = min(dSpeed,ServiceCarStatus.mroadmode_vel.mfmode18);
+            lock_flag = true;
+        }
+    }
+}
+
+float iv::decition::DecideGps00::computeAvoidX(int roadAim,int roadOri,GPSData gps)
+{
+    if(roadAim==roadOri)
+    {
+        return 0;
+    }
+    float x = 0;
+    float veh_to_roadSide = (gps->mfLaneWidth - ServiceCarStatus.msysparam.vehWidth) * 0.5;
+    float roadSide_to_roadSide = ServiceCarStatus.msysparam.vehWidth;
+    if(!ServiceCarStatus.inRoadAvoid)
+    {
+        x = (roadOri-roadAim) * gps->mfLaneWidth;
+    }
+    else
+    {
+        int num = roadOri - roadAim;
+        switch (abs(num) % 3)
+        {
+        case 0:
+            x = (num / 3) * gps->mfLaneWidth;
+            break;
+        case 1:
+            if(num > 0)
+            {
+                x = (num / 3) * gps->mfLaneWidth + veh_to_roadSide;
+            }
+            else
+            {
+                x = (num / 3) * gps->mfLaneWidth - veh_to_roadSide;
+            }
+            break;
+        case 2:
+            if(num > 0)
+            {
+                x = (num / 3) * gps->mfLaneWidth + veh_to_roadSide + roadSide_to_roadSide;
+            }
+            else
+            {
+                x = (num / 3) * gps->mfLaneWidth - veh_to_roadSide - roadSide_to_roadSide;
+            }
+
+            break;
+        default:
+            break;
+        }
+    }
+    return x;
+}
+
+double iv::decition::DecideGps00::computeDistToEnd(const std::vector<GPSData> gpsMapLine,int pathpoint)
+{
+    double dist_to_end = 0;
+    for(int i = pathpoint;i < gpsMapLine.size() - 1; i++)
+    {
+        if(gpsMapLine[i]->mode2 != 23)
+            dist_to_end += GetDistance(*gpsMapLine[i+1], *gpsMapLine[i]);
+    }
+    return dist_to_end;
+}
+
+void iv::decition::DecideGps00::computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,int* avoidXLeft, int* avoidXRight )
+{
+    double RightBoundary = (((double)roadOriginal + 0.5) * roadWidthEvery - vehicleWidth / 2);
+    *avoidXRight = ((int)RightBoundary);
+    double LeftBoundary = (((double)(roadSumMap -roadOriginal - 1) + 0.5) * roadWidthEvery - vehicleWidth / 2);
+    *avoidXLeft = (-(int)LeftBoundary);
+}
+
+void iv::decition::DecideGps00::computeAvoidBoundary(int roadOriginal,int roadSumMap,double roadWidthEvery,double vehicleWidth,double* avoidXLeft, double* avoidXRight )
+{
+    double RightBoundary = (((double)roadOriginal + 0.5) * roadWidthEvery - vehicleWidth / 2);
+    *avoidXRight = ((int)RightBoundary);
+    double LeftBoundary = (((double)(roadSumMap -roadOriginal - 1) + 0.5) * roadWidthEvery - vehicleWidth / 2);
+    *avoidXLeft = (-(int)LeftBoundary);
+}
+
+int iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,
+                                                     int avoidLeft,int avoidRight,int offsetLast)
+{
+    int signed_record_avoidX = offsetLast, record_avoidX = 20;
+    double obs_cur_distance =500, record_obstacle_distance;
+    iv::Point2D obs_spline_record;
+    double step = 0.5; //lyp
+
+    obs_property.clear();
+    for (double i =  avoidLeft; i <= avoidRight; i=i+step)  //由原来的1米改成0.5米，将i由int 转换成double
+    {
+        obsvalue x_value;
+        obsvalue *x = &x_value;
+        x_value.avoid_distance = i;
+        gpsTraceAvoid.clear();
+        gpsTraceAvoid = getGpsTraceOffset(gpsTraceMapOri, i);
+        obs_spline_record=computeObsOnRoadSpline(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,now_gps_ins,x);
+        obs_property.push_back(x_value);
+        if(i==offsetLast)
+        {
+            obs_cur_distance=x_value.obs_distance;
+            record_obstacle_distance=obs_cur_distance;
+        }
+        givlog->debug("decition_brain","i: %d,obs:%f",x_value.avoid_distance,x_value.obs_distance);
+
+        obsSpline.push_back(obs_spline_record);
+    }
+
+    if (lidarGridPtr != NULL)
+    {
+        hasCheckedAvoidLidar = true;
+    }
+    if(offsetLast==0)
+    {
+        unsigned int vector_num_record=0;
+        for(int j = 0; j < obs_property.size(); j++)
+        {
+            if(obs_property[j].obs_distance > 100)
+            {
+                if(abs(obs_property[j].avoid_distance) < record_avoidX)
+                {
+                    record_avoidX = abs(obs_property[j].avoid_distance);
+                    signed_record_avoidX = obs_property[j].avoid_distance;
+                    vector_num_record = j;
+                }
+            }
+        }
+        if((signed_record_avoidX < 0) && (signed_record_avoidX-1 >= avoidLeft))
+        {
+            //int obs_test=abs(signed_record_avoidX)-1;
+            int obs_test = vector_num_record-1;
+            if(obs_property[obs_test].obs_distance > 100)
+            {
+                signed_record_avoidX -= 1;
+                givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+            }
+        }
+        //added by lyp
+        // if((signed_record_avoidX < 0) && (signed_record_avoidX-1 >= avoidLeft))
+        // {
+        //     //int obs_test=abs(signed_record_avoidX)-1;
+        //     int obs_test = vector_num_record-1;
+        //     if(obs_property[obs_test].obs_distance > 100)
+        //     {
+        //         signed_record_avoidX -= 1;
+        //         givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+        //     }
+        // }
+
+        if(signed_record_avoidX == 0)
+        {
+            for(int j = 0; j < obs_property.size(); j++)
+            {
+                if(obs_property[j].obs_distance > obs_cur_distance+15)
+                {
+                    if(obs_property[j].obs_distance > record_obstacle_distance)
+                    {
+                        record_avoidX = abs(obs_property[j].avoid_distance);
+                        signed_record_avoidX = obs_property[j].avoid_distance;
+                        record_obstacle_distance = obs_property[j].obs_distance;
+                    }
+                }
+            }
+        }
+    }
+    else
+    {
+        for(int j = 0; j < obs_property.size();j++)
+        {
+            if(obs_property[j].obs_distance > 100)
+            {
+                if((abs(obs_property[j].avoid_distance)<record_avoidX)&&(obs_property[j].avoid_distance!=offsetLast))
+                {
+                    record_avoidX=abs(obs_property[j].avoid_distance);
+                    signed_record_avoidX=obs_property[j].avoid_distance;
+                }
+            }
+        }
+        if(signed_record_avoidX==0)
+        {
+            return 0;
+        }
+        else if(obs_cur_distance>30)
+        {
+            signed_record_avoidX = offsetLast;
+        }
+        else if((obs_cur_distance<=30) && (signed_record_avoidX==offsetLast))
+        {
+            for(int j = 0;j<obs_property.size();j++)
+            {
+                if(obs_property[j].obs_distance>obs_cur_distance+15)
+                {
+                    if(obs_property[j].obs_distance>record_obstacle_distance)
+                    {
+                        record_avoidX = abs(obs_property[j].avoid_distance);
+                        signed_record_avoidX = obs_property[j].avoid_distance;
+                        record_obstacle_distance = obs_property[j].obs_distance;
+                    }
+                }
+            }
+        }
+    }
+
+    if((signed_record_avoidX!=offsetLast)&&(obs_cur_distance<50))
+    {
+        avoidMinDistance = obs_cur_distance + 2*ServiceCarStatus.msysparam.vehLenth;
+        startAvoidGps = now_gps_ins;
+        return signed_record_avoidX;
+    }
+    return offsetLast;
+}
+
+double iv::decition::DecideGps00::computeAvoidXDistance(iv::LidarGridPtr lidarGridPtr, GPS_INS now_gps_ins, const std::vector<GPSData> gpsMapLine,
+                                                     double avoidLeft,double avoidRight,double offsetLast)
+{
+    double signed_record_avoidX = offsetLast, record_avoidX = 20;
+    double obs_cur_distance =500, record_obstacle_distance;
+    iv::Point2D obs_spline_record;
+    double step = 0.5;
+
+    obs_property.clear();
+    for (double i =  avoidLeft; i <= avoidRight; i=i+step)  //由原来的1米改成0.5米，将i由int 转换成double
+    {
+        obsvalue x_value;
+        obsvalue *x = &x_value;
+        x_value.avoid_distance = i;
+        gpsTraceAvoid.clear();
+        gpsTraceAvoid = getGpsTraceOffset(gpsTraceMapOri, i);
+        obs_spline_record=computeObsOnRoadSpline(lidarGridPtr, gpsTraceAvoid, i,gpsMapLine,now_gps_ins,x);
+        obs_property.push_back(x_value);
+        if(i==offsetLast)
+        {
+            obs_cur_distance=x_value.obs_distance;
+            record_obstacle_distance=obs_cur_distance;
+        }
+        givlog->debug("decition_brain","i: %d,obs:%f",x_value.avoid_distance,x_value.obs_distance);
+
+        obsSpline.push_back(obs_spline_record);
+    }
+
+    if (lidarGridPtr != NULL)
+    {
+        hasCheckedAvoidLidar = true;
+    }
+    if(offsetLast==0)//上次的避障距离是0或者沿参考线行驶第一次避障
+    {
+        unsigned int vector_num_record=0;
+        for(int j = 0; j < obs_property.size(); j++)
+        {
+            if(obs_property[j].obs_distance > 100) //障碍物距离大于100米认为可以臂章
+            {
+                if(abs(obs_property[j].avoid_distance) < record_avoidX)
+                {
+                    record_avoidX = abs(obs_property[j].avoid_distance);    //找到距离车辆当前行驶的位置横向最近的臂章距离
+                    signed_record_avoidX = obs_property[j].avoid_distance;
+                    vector_num_record = j;
+                }
+            }
+        }
+        //优先向左避障
+        if((signed_record_avoidX < 0) && (signed_record_avoidX-2*step>= avoidLeft))  //-0.5  -1  -1.5都可以避障的时候选则-1.5,
+        {
+            //int obs_test=abs(signed_record_avoidX)-1;
+            int obs_test = vector_num_record-1;
+            int obs_testneighbor = obs_test-1;
+            if((obs_property[obs_test].obs_distance > 100)&&(obs_property[obs_testneighbor].obs_distance>100))
+            {
+                signed_record_avoidX =signed_record_avoidX-2*step;
+                givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+            }
+        }
+        else if((signed_record_avoidX < 0) && (signed_record_avoidX-1*step>= avoidLeft))//-0.5  -1 可以避障的时候选则-1,
+        {
+            //int obs_test=abs(signed_record_avoidX)-1;
+            int obs_test = vector_num_record-1;
+            if(obs_property[obs_test].obs_distance > 100)
+            {
+                signed_record_avoidX -= 1;
+                givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+            }
+        }
+        else if((signed_record_avoidX > 0) && (signed_record_avoidX+2*step<= avoidRight))  //0.5  1  1.5都可以避障的时候选则1.5,
+        {
+            int obs_test = vector_num_record+1;
+            int obs_testneighbor = obs_test+1;
+            if((obs_property[obs_test].obs_distance > 100)&&(obs_property[obs_testneighbor].obs_distance>100)
+                    )
+            {
+                signed_record_avoidX =signed_record_avoidX+2*step;
+                givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+            }
+        }
+        else if((signed_record_avoidX>0) && (signed_record_avoidX+1*step<= avoidRight))//0.5  1 可以避障的时候选则1,
+        {
+            int obs_test = vector_num_record+1;
+            if(obs_property[obs_test].obs_distance > 100)
+            {
+                signed_record_avoidX -= 1;
+                givlog->debug("decition_brain","singrecord_Y: %d",signed_record_avoidX);
+            }
+        }
+        else if (signed_record_avoidX < 0)
+        {
+                signed_record_avoidX=signed_record_avoidX; //只有-0.5 0.5 的时候就选则-0.5  0.5,优先选择-0.5
+        }
+        else
+        {
+            ;
+        }
+
+        if(signed_record_avoidX == 0)
+        {
+            for(unsigned int j = 0; j < obs_property.size(); j++)
+            {
+                if(obs_property[j].obs_distance > obs_cur_distance+15)
+                {
+                    if(obs_property[j].obs_distance > record_obstacle_distance)
+                    {
+                        record_avoidX = abs(obs_property[j].avoid_distance);
+                        signed_record_avoidX = obs_property[j].avoid_distance;
+                        record_obstacle_distance = obs_property[j].obs_distance;
+                    }
+                }
+            }
+            givlog->debug("decition_brain","offsetLast==0,and signed_record_avoidX==0");//20230213
+        }
+    }
+    else
+    {
+        for(int j = 0; j < obs_property.size();j++)
+        {
+            if(obs_property[j].obs_distance > 100)
+            {
+                if((abs(obs_property[j].avoid_distance)<record_avoidX)&&(obs_property[j].avoid_distance!=offsetLast))
+                {
+                    record_avoidX=abs(obs_property[j].avoid_distance);
+                    signed_record_avoidX=obs_property[j].avoid_distance;//返回的是距离原始参考路线最近的路线
+                }
+            }
+        }
+        if(signed_record_avoidX==0) //优先返回原参考路线
+        {
+            return 0;
+        }
+        else if(obs_cur_distance>30)//如果当前障碍物距离大于30米，继续当前的避障距离
+        {
+            signed_record_avoidX = offsetLast;
+        }
+        else if((obs_cur_distance<=30) && (signed_record_avoidX==offsetLast))
+        {
+            for(unsigned int j = 0;j<obs_property.size();j++)
+            {
+                if(obs_property[j].obs_distance>obs_cur_distance+15)
+                {
+                    if(obs_property[j].obs_distance>record_obstacle_distance)
+                    {
+                        record_avoidX = abs(obs_property[j].avoid_distance);
+                        signed_record_avoidX = obs_property[j].avoid_distance;
+                        record_obstacle_distance = obs_property[j].obs_distance;
+                    }
+                }
+            }
+            givlog->debug("decition_brain","obs_cur_distance<=30and signed_record_avoidX==offsetLast");//20230213
+        }
+    }
+
+    if((signed_record_avoidX!=offsetLast)&&(obs_cur_distance<50))
+    {
+        avoidMinDistance = obs_cur_distance + 2*ServiceCarStatus.msysparam.vehLenth;
+        startAvoidGps = now_gps_ins;
+        return signed_record_avoidX;
+    }
+    return offsetLast;
+}
+
+//iv::Point2D iv::decition::DecideGps00::computeObsOnRoadSpline(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,
+//                                                              const std::vector<GPSData> gpsMapLine,GPS_INS now_gps_ins,obsvalue* obsva)
+iv::Point2D iv::decition::DecideGps00::computeObsOnRoadSpline(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, double roadNum,
+                                                              const std::vector<GPSData> gpsMapLine,GPS_INS now_gps_ins,obsvalue* obsva) //20230213
+{
+    double obs=-1,obsSd=-1;
+    double obsCarCoordinateX,obsCarCoordinateY;
+    GPS_INS obsGeodetic;
+    Point2D obsFrenet,obsFrenetMid;
+    double obsCarCoordinateDistance=-1;
+
+    if (lidarGridPtr == NULL)
+    {
+        lidarDistance = lastLidarDis;
+    }
+    else
+    {
+        obsPoint = Compute00().getLidarObsPoint(gpsTrace, lidarGridPtr);
+        obsCarCoordinateDistance=obsPoint.y;
+
+        if((obsCarCoordinateDistance > 0) && (obsCarCoordinateDistance < 100))
+        {
+            obsCarCoordinateX = obsPoint.x;
+            obsCarCoordinateY = obsPoint.y;
+            obsGeodetic  = Coordinate_UnTransfer(obsCarCoordinateX, obsCarCoordinateY, now_gps_ins);      //车体转大地
+            obsFrenetMid = cartesian_to_frenet1D(gpsMapLine,obsGeodetic.gps_x,obsGeodetic.gps_y);          //大地转frenet
+            iv::Point2D now_s_d = cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
+            givlog->debug("decition_brain","road_num: %d,obsFrenetMid_s: %f,now_s: %f",roadNum,obsFrenetMid.s,now_s_d.s);
+            obsFrenet.s=obsFrenetMid.s-now_s_d.s;
+            obsFrenet.d=roadNum;
+
+            //test
+            //obsFrenet.s=obsPoint.y;
+            //obsFrenet.d=roadNum;
+        }
+        else
+        {
+            obsFrenet.s=500;
+            obsFrenet.d=roadNum;
+        }
+
+        if (obsFrenet.s<0)
+        {
+            obsFrenet.s=0;
+        }
+        lidarDistance = obsFrenet.s;
+
+        lastLidarDis = lidarDistance;
+    }
+
+    obs = lidarDistance;
+    obsSd = obsPoint.obs_speed_y;
+    obsva->obs_distance = obs;
+    obsva->obs_speed = obsSd;
+
+    return obsFrenet;
+}

src/decition/decition_brain_sf_changan_shenlan/decition/decition.pri

@@ -30,7 +30,8 @@ HEADERS += \
     $$PWD/adc_adapter/interpolation_2d.h \
     $$PWD/adc_tools/PolynomialRegression.h \
     $$PWD/adc_tools/polyfit.h \
-    $$PWD/adc_planner/spline_planner.h
+    $$PWD/adc_planner/spline_planner.h \
+    $$PWD/adc_adapter/hunter_adapter.h
 
 SOURCES += \
     $$PWD/adc_adapter/changan_shenlan_adapter.cpp \
@@ -60,4 +61,5 @@ SOURCES += \
     $$PWD/adc_adapter/sightseeing_adapter.cpp \
     $$PWD/adc_adapter/interpolation_2d.cc \
     $$PWD/adc_tools/polyfit.cpp \
-    $$PWD/adc_planner/spline_planner.cpp
+    $$PWD/adc_planner/spline_planner.cpp \
+    $$PWD/adc_adapter/hunter_adapter.cpp

src/decition/decition_brain_sf_changan_shenlan/decition_brain_sf_changan_shenlan.pro

@@ -2,7 +2,7 @@ QT -= gui
 
 QT += network dbus xml widgets
 
-CONFIG += c++11 console
+CONFIG += c++11  #console
 CONFIG -= app_bundle
 
 # The following define makes your compiler emit warnings if you use
@@ -36,7 +36,6 @@ SOURCES += $$PWD/../common/main.cpp \
     ../../include/msgtype/v2r.pb.cc \
     ../../include/msgtype/groupmsg.pb.cc
 
-
 include($$PWD/../common/common/common.pri)
 include($$PWD/decition/decition.pri)
 include($$PWD/../common/perception_sf/perception_sf.pri)
@@ -97,8 +96,7 @@ HEADERS += \
     ../common/perception_sf/sensor_lidar.h \
     ../common/perception_sf/sensor_radar.h \
     ../common/common/sysparam_type.h \
-    ../../include/msgtype/carstate.pb.h \
-
+    ../../include/msgtype/carstate.pb.h
     ../../include/msgtype/v2r.pb.h \
 
     ../../include/msgtype/groupmsg.pb.h

src/decition/decition_brain_sf_changan_shenlan_vedio/mainwindow.ui

@@ -1,24 +0,0 @@
-<ui version="4.0" >
- <author></author>
- <comment></comment>
- <exportmacro></exportmacro>
- <class>MainWindow</class>
- <widget class="QMainWindow" name="MainWindow" >
-  <property name="geometry" >
-   <rect>
-    <x>0</x>
-    <y>0</y>
-    <width>800</width>
-    <height>600</height>
-   </rect>
-  </property>
-  <property name="windowTitle" >
-   <string>MainWindow</string>
-  </property>
-  <widget class="QMenuBar" name="menubar" />
-  <widget class="QWidget" name="centralwidget" />
-  <widget class="QStatusBar" name="statusbar" />
- </widget>
- <pixmapfunction></pixmapfunction>
- <connections/>
-</ui>

src/detection/detection_chassis/decodechassis.cpp

@@ -1,6 +1,7 @@
 
 #include "decodechassis.h"
 #include <iostream>
+#include <chrono>
 #include "ivlog.h"
 #include "ivfault.h"
 
@@ -12,6 +13,27 @@ extern "C"
 iv::Ivlog * mygivlog;
 iv::Ivfault * mygivfault;
 
+
+void PrintCANRaw(const iv::can::canraw * praw )
+{
+    char strout[512];
+    int ndatalen = praw->len();
+    int i;
+    snprintf(strout,512,"%ld id: %02X data: ",std::chrono::system_clock::now().time_since_epoch().count()/1000000, praw->id());
+    char * pdata = new char[8];
+    if(ndatalen>0)pdata = new char[ndatalen];
+    memcpy(pdata,praw->data().data(),ndatalen);
+    for(i=0;i<ndatalen;i++)
+    {
+        char strtem[10];
+        snprintf(strtem,10,"%02X ",pdata[i]);
+        strncat(strout,strtem,512);
+    }
+    std::cout<<strout<<std::endl;
+    delete  pdata;
+
+}
+
 /**
   * @brief ShareChassis
   * Share Chassis State
@@ -489,6 +511,7 @@ int ProcShenLanCANFDChassis(void *pa, iv::can::canmsg *pmsg)
 
                 xchassis.set_vel(static_cast<float>(vehspeed));
                 ShareChassis(pa,&xchassis);
+                xchassis.clear_angle_feedback();
                 std::cout<<"veh: "<<xchassis.vel()<<std::endl;
             }
             else
@@ -512,7 +535,7 @@ int ProcShenLanCANFDChassis(void *pa, iv::can::canmsg *pmsg)
                     ang=static_cast<double>(value) * 0.1-65536*0.1;
 
                 xchassis.set_angle_feedback(static_cast<float>(ang));
-                ShareChassis(pa,&xchassis);
+     //           ShareChassis(pa,&xchassis);
                 std::cout<<"ang: "<<xchassis.angle_feedback()<<std::endl;
             }
             else
@@ -526,3 +549,139 @@ int ProcShenLanCANFDChassis(void *pa, iv::can::canmsg *pmsg)
 
     return 0;
 }
+
+int ProcHunterChassis(void *pa, iv::can::canmsg *pmsg)
+{
+    int i;
+    static iv::chassis xchassis;
+    static bool bHave0x221 = false;
+    static bool bHave0x361 = false;
+    float fsoc = 0.0;
+    float fsoh = 0.0;
+    float fbatv = 0.0;
+    float fbati = 0.0;
+    float fbatt = 0.0;
+    float fmileage_left = 0.0;
+    float fmileage_right = 0.0;
+    double fwheelangle_feedback = 0.0;
+    for(i=0;i<pmsg->rawmsg_size();i++)
+    {
+
+        const iv::can::canraw * praw = &(pmsg->rawmsg(i));
+//        unsigned char data[8];
+//        memcpy(data,praw->data().data(),8);
+        if(praw->id() == 0x221)
+        {
+            unsigned char vdata[2];
+            memcpy(&vdata,praw->data().data(),2);
+            unsigned char vswap[2];
+            vswap[0] = vdata[1];
+            vswap[1] = vdata[0];
+            short svalue;
+            memcpy(&svalue,vswap,2);
+            double vehspeed =  svalue;
+            vehspeed = vehspeed * 0.001 *3.6;
+
+            memcpy(&vdata,praw->data().data() + 6,2);
+            vswap[0] = vdata[1];
+            vswap[1] = vdata[0];
+            memcpy(&svalue,vswap,2);
+            fwheelangle_feedback =  svalue;
+            fwheelangle_feedback = fwheelangle_feedback * 0.001;
+
+
+            xchassis.set_vel(static_cast<float>(vehspeed));
+            xchassis.set_angle_feedback(fwheelangle_feedback);
+            bHave0x221 = true;
+        }
+        if(praw->id() == 0x361)
+        {
+            unsigned char vdata[8];
+            memcpy(&vdata,praw->data().data(),8);
+ //           std::cout<<" vdata "<<(int)vdata[0]<<" "<<(int)vdata[1]<<" "<<(int)vdata[2]<<" "<<(int)vdata[3]<<" "<<std::endl;
+            fsoc = vdata[0];
+            memcpy(&vdata,praw->data().data() + 1,1);
+            fsoh = vdata[0];
+            memcpy(&vdata,praw->data().data()+2,2);
+            double fvh = vdata[0];
+            double fvl = vdata[1];
+            fbatv = (fvh * 256 + fvl) * 0.01f;
+
+            memcpy(&vdata,praw->data().data()+4,2);
+            unsigned char vswap[2];
+            vswap[0] = vdata[1];
+            vswap[1] = vdata[0];
+            unsigned short svalue;
+            memcpy(&svalue,vswap,2);
+            fbati = svalue;
+            fbati = fbati * 0.1f;
+
+            memcpy(&vdata,praw->data().data()+6,2);
+            vswap[0] = vdata[1];
+            vswap[1] = vdata[0];
+            memcpy(&svalue,vswap,2);
+            fbatt = svalue;
+            fbatt = fbatt * 0.1f;
+
+            bHave0x361 = true;
+        }
+        if(praw->id() == 0x311)
+        {
+            unsigned char vdata[4];
+            memcpy(&vdata,praw->data().data(),4);
+            unsigned char vswap[4];
+            vswap[0] = vdata[3];
+            vswap[1] = vdata[2];
+            vswap[2] = vdata[1];
+            vswap[3] = vdata[0];
+            int mil_left;
+            memcpy(&mil_left,vswap,4);
+            fmileage_left = mil_left;
+            fmileage_left = fmileage_left * 0.001f;
+
+            memcpy(&vdata,praw->data().data(),4);
+            vswap[0] = vdata[3];
+            vswap[1] = vdata[2];
+            vswap[2] = vdata[1];
+            vswap[3] = vdata[0];
+            int mil_right;
+            memcpy(&mil_right,vswap,4);
+            fmileage_right = mil_right;
+            fmileage_right = fmileage_right * 0.001f;
+        }
+        if(praw->id() == 0x358)
+        {
+
+
+        }
+        if(praw->id() == 0x3AA)
+        {
+
+
+        }
+
+        if(bHave0x221 && bHave0x361)
+        {
+            bHave0x221 = false;
+            bHave0x361 = false;
+            ShareChassis(pa,&xchassis);
+
+            std::cout<<"veh: "<<xchassis.vel()<<" wheel: "<<fwheelangle_feedback<<" soc: "<<fsoc<<" soh:"<<fsoh<< std::endl;
+            std::cout<<"bat v:"<<fbatv<<" i: "<<fbati<<" Temp:"<<fbatt<<" mil left:"<<fmileage_left<<" right: "<<fmileage_right<<std::endl;
+        }
+
+//        if(ghave0x13&&ghave0x14&&ghave0x15)
+//        {
+//            ghave0x13 = false;
+//            ghave0x14 = false;
+//            ghave0x15 = false;
+//            ShareChassis(pa,&xchassis);
+//            nRtn = 1;
+//        }
+    }
+
+
+    return 0;
+}
+
+

src/detection/detection_chassis/decodechassis.h

@@ -26,4 +26,8 @@ int ProcShenLanChassis(void * pa, iv::can::canmsg * pmsg);
 
 int ProcShenLanCANFDChassis(void *pa, iv::can::canmsg *pmsg);
 
+int ProcHunterChassis(void *pa, iv::can::canmsg *pmsg);
+
+void PrintCANRaw(const iv::can::canraw * praw );
+
 #endif // DECODECHASSIS_H

src/detection/detection_chassis/main.cpp

@@ -24,10 +24,12 @@ void * gpa ;
 
 QTime gTime;
 
+bool gbPrintRaw = false;
+
 
 namespace  iv {
 
-enum VehicleTypeDef {GE3,VV7,MIDCAR,PROBLUE,MIDBUS,HAPO,UNKNOWN, YUHESEN,SHENLAN,SHENLAN_CANFD} gVehicleType;  //车辆类型
+enum VehicleTypeDef {GE3,VV7,MIDCAR,PROBLUE,MIDBUS,HAPO,UNKNOWN, YUHESEN,SHENLAN,SHENLAN_CANFD,HUNTER} gVehicleType;  //车辆类型
 
 
 }
@@ -61,6 +63,16 @@ void Listencan0(const char * strdata,const unsigned int nSize,const unsigned int
         return;
     }
 
+    if(gbPrintRaw)
+    {
+        int i;
+        for(i=0;i<xmsg.rawmsg_size();i++)
+        {
+            const iv::can::canraw * praw = &(xmsg.rawmsg(i));
+            PrintCANRaw(praw);
+        }
+    }
+
     int nRtn = 0;
     switch (iv::gVehicleType) {
     case iv::GE3:
@@ -89,6 +101,9 @@ void Listencan0(const char * strdata,const unsigned int nSize,const unsigned int
     case iv::SHENLAN_CANFD:
         nRtn = ProcShenLanCANFDChassis(gpa,&xmsg);
         break;
+    case iv::HUNTER:
+        nRtn = ProcHunterChassis(gpa,&xmsg);
+        break;
     default:
         break;
     }
@@ -154,6 +169,7 @@ int main(int argc, char *argv[])
     std::string strvehicletype = xp.GetParam("vehicletype","GE3");
     std::string strmodulename = xp.GetParam("modulename","chassis"); //chassismsgname
     std::string strchassismsgname = xp.GetParam("chassismsgname","chassis");
+    gbPrintRaw = xp.GetParam("PrintRaw",false);
 
     iv::gVehicleType = iv::UNKNOWN;
     if(strncmp(strvehicletype.data(),"GE3",255) == 0)
@@ -197,6 +213,11 @@ int main(int argc, char *argv[])
         iv::gVehicleType = iv::SHENLAN_CANFD;
     }
 
+   if(strncmp(strvehicletype.data(),"HUNTER",255) == 0)
+    {
+        iv::gVehicleType = iv::HUNTER;
+    }
+
 //iv::gVehicleType = iv::MIDBUS;
 
     givlog = new iv::Ivlog(strmodulename.data());

src/detection/detection_lidar_grid/detection_lidar_grid.pro

@@ -6,13 +6,14 @@
 
 QT       -= gui
 
-CONFIG += c++11 console
+CONFIG += c++14 console
 CONFIG -= app_bundle
 
 DEFINES += PERCEPTION_LIDAR_VV7_LIBRARY
 
 #CONFIG += plugin
 
+
 # The following define makes your compiler emit warnings if you use
 # any feature of Qt which has been marked as deprecated (the exact warnings
 # depend on your compiler). Please consult the documentation of the
@@ -58,11 +59,15 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 SOURCES += \
         perception_lidar_vv7.cpp \
-    perceptionoutput.cpp
+    perceptionoutput.cpp \
+    ../../include/msgtype/fusionobject.pb.cc \
+    ../../include/msgtype/fusionobjectarray.pb.cc
 
 HEADERS += \
         perception_lidar_vv7.h \
-    perceptionoutput.h
+    perceptionoutput.h \
+    ../../include/msgtype/fusionobject.pb.h \
+    ../../include/msgtype/fusionobjectarray.pb.h
 
 unix {
     target.path = /usr/lib

src/detection/detection_lidar_grid/perception_lidar_vv7.cpp

@@ -35,6 +35,8 @@ static void * g_lidar_pc_nofloor;
 static void * glidar_pc_p;
 static void * glidar_per;
 
+static void * gpafusion;
+
 iv::Ivfault *gfault = nullptr;
 iv::Ivlog *givlog = nullptr;
 
@@ -101,7 +103,9 @@ static void writepctosm(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,void
     int * pHeadSize = (int *)strOut;
     *pHeadSize = 4 + point_cloud->header.frame_id.size()+4+8;
     memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
-    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+//    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+
+    unsigned int * pu32 = (unsigned int *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
     *pu32 = point_cloud->header.seq;
     memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
     pcl::PointXYZI * p;
@@ -118,8 +122,8 @@ static void writepctosm(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,void
 
 
 
-static float SENSOR_HEIGHT = 2.1;
-static float VEHICLE_HEIGHT = 2.6;
+static float SENSOR_HEIGHT = 1.9;
+static float VEHICLE_HEIGHT = 2.2;
 //static float local_max_slope_ = 10;
 //static float general_max_slope_ = 3;
 //static float concentric_divider_distance_ = 1.5;
@@ -278,7 +282,7 @@ static void GridGetObs(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,std::s
         Cpos = (x-HOR_MIN)/GRID_SIZE;
         if((Rpos>=0)&&(Rpos<nVecSize)&&(Cpos>=0)&&(Cpos<nHorSize))
         {
-            if((z-floor_h[Rpos*nHorSize+Cpos])>0.3)
+            if((z-floor_h[Rpos*nHorSize+Cpos])>0.15)
             {
                point_cloud_nofloor->push_back(po);
                ++point_cloud_nofloor->width;
@@ -400,6 +404,59 @@ static void GridGetObs(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,std::s
 
 #include <QTime>
 
+#include "fusionobjectarray.pb.h"
+
+iv::fusion::fusionobjectarray xfusion;
+
+void ShareFusionObject(void * pa ,std::shared_ptr<std::vector<iv::ObstacleBasic>> & lidar_obs)
+{
+
+    double timestamp = std::chrono::system_clock::now().time_since_epoch().count();
+    timestamp = timestamp/1000000.0;
+
+    xfusion.clear_obj();
+
+    xfusion.set_timestamp(timestamp);
+
+    iv::fusion::fusionobject * pobj =  xfusion.add_obj();
+
+    pobj->set_id(0);
+    pobj->set_type(0);
+    iv::fusion::VelXY *pxvel = new iv::fusion::VelXY;
+    pxvel->set_x(0);
+    pxvel->set_y(0);
+    pobj->set_allocated_vel_abs(pxvel);
+    iv::fusion::Dimension * pxdim = new iv::fusion::Dimension;
+    pxdim->set_x(0.2);
+    pxdim->set_y(0.2);
+    pxdim->set_z(2.0);
+
+    pobj->set_allocated_dimensions(pxdim);
+
+    int i;
+    int nsize = lidar_obs->size();
+    for(i=0;i<nsize;i++)
+    {
+        iv::fusion::NomalXYZ * pcen = pobj->add_nomal_centroid();
+        pcen->set_nomal_x(lidar_obs->at(i).nomal_x);
+        pcen->set_nomal_y(lidar_obs->at(i).nomal_y);
+        pcen->set_nomal_z(lidar_obs->at(i).nomal_z);
+    }
+
+    int nbytesize = xfusion.ByteSize();
+    std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
+
+    if(xfusion.SerializeToArray(pstr_ptr.get(),nbytesize))
+    {
+        iv::modulecomm::ModuleSendMsg(pa,pstr_ptr.get(),nbytesize);
+    }
+    else
+    {
+        std::cout<<" serialzie fusion fail."<<std::endl;
+    }
+
+
+}
 
 
 static void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
@@ -446,6 +503,8 @@ static void ListenPointCloud(const char * strdata,const unsigned int nSize,const
 
 //   std::cout<<"prc."<<std::endl;
 
+    ShareFusionObject(gpafusion,lidar_obs);
+
     if(g_robosys == 0)
     {
         if(lidar_obs->size() == 0)
@@ -484,6 +543,8 @@ void  PERCEPTION_LIDAR_VV7SHARED_EXPORT StartPerception_lidar_vv7()
 
     glidar_obs = iv::modulecomm::RegisterSend(gstr_outputmemname,20000000,3);
 
+    gpafusion = iv::modulecomm::RegisterSend("li_ra_fusion",10000000,1);
+
 #ifdef OUT_GROUND
     g_lidar_pc_floor = iv::modulecomm::RegisterSend("lidar_pc_floor",20000000,3);
     g_lidar_pc_nofloor = iv::modulecomm::RegisterSend("lidar_pc_nofloor",20000000,3);

src/detection/detection_ndt_matching/detection_ndt_matching.pro

@@ -0,0 +1,92 @@
+QT -= gui
+QT += network
+CONFIG += c++17 #console
+CONFIG -= app_bundle
+
+QMAKE_CXXFLAGS += -std=gnu++17
+
+QMAKE_LFLAGS += -no-pie
+
+QMAKE_CXXFLAGS +=  -g
+
+QMAKE_CXXFLAGS += -O3
+
+# 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 \
+    globalrelocation.cpp
+
+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 \
+    globalrelocation.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!" )
+}
+
+INCLUDEPATH += $$PWD/../../common/ndt_cpu/
+
+
+#DEFINES+= TEST_CALCTIME
+
+INCLUDEPATH += $$PWD/../../../include/
+LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault -lndt_cpu  -livexit -livbacktrace
+
+

src/detection/detection_ndt_matching/detection_ndt_matching.xml

@@ -0,0 +1,14 @@
+<xml>	
+	<node name="detection_ndt_matching.xml">
+		<param name="lidarmsg" value="lidar_pc" />
+		<param name="gpsmsg" value="hcp2_gpsimu" />
+		<param name="ndtposmsg" value="ndtpos" />
+		<param name="ndtgpsposmsg" value="ndtgpspos" />
+		<param name="AngleCorrect" value="0.0" />
+		<param name="HeightCorrect" value="0.0" />
+		<param name="SaveLastPos" value="true" />
+		<param name="Arm_LidarGPSBase_x" value="0.0" />
+		<param name="Arm_LidarGPSBase_y" value="0.0" />
+		<param name="useanh" value="false" />
+	</node>
+</xml>

src/detection/detection_ndt_matching/globalrelocation.cpp

@@ -0,0 +1,484 @@
+#include "globalrelocation.h"
+
+#include <pcl/filters/voxel_grid.h>
+#include <tf/tf.h>
+#include <pcl/io/pcd_io.h>
+
+GlobalRelocation::GlobalRelocation()
+{
+//    std::cout<<" run hear."<<std::endl;
+    if(mnThreadNum > MAX_NDT)mnThreadNum = MAX_NDT;
+    int i;
+    for(i=0;i<MAX_NDT;i++)mpthread[i] = NULL;
+}
+
+void GlobalRelocation::threadtest(int n,iv::Reloc & xReloc)
+{
+
+}
+
+void GlobalRelocation::threadReloc(int nThread,cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> * pndt,
+                                   pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan,
+                                   iv::Reloc  xReloc)
+{
+
+    xReloc.x_calc = xReloc.x_guess;
+    xReloc.y_calc = xReloc.y_guess;
+    xReloc.z_calc = xReloc.z_guess;
+    xReloc.yaw_calc = xReloc.yaw_guess;
+    xReloc.pitch_calc = 0;// xReloc.pitch_guess;
+    xReloc.roll_calc = 0;//xReloc.roll_guess;
+    xReloc.trans_prob = 0;
+
+
+    Eigen::Matrix4f tf_btol;
+    Eigen::Translation3f tl_btol(0, 0, 0);                 // tl: translation
+    Eigen::AngleAxisf rot_x_btol(0, Eigen::Vector3f::UnitX());  // rot: rotation
+    Eigen::AngleAxisf rot_y_btol(0, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf rot_z_btol(0, Eigen::Vector3f::UnitZ());
+    tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
+
+    double fLastProb = 0.0;
+    int i;
+    for(i=0;i<50;i++)
+    {
+
+        double voxel_leaf_size = 2.0;
+
+        pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan(new pcl::PointCloud<pcl::PointXYZ>());
+
+        pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
+        voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
+        voxel_grid_filter.setInputCloud(raw_scan);
+        voxel_grid_filter.filter(*filtered_scan);
+
+        pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(*filtered_scan));
+
+        pndt->setInputSource(filtered_scan_ptr);
+
+        Eigen::Translation3f init_translation(xReloc.x_calc,xReloc.y_calc, xReloc.z_calc);
+        Eigen::AngleAxisf init_rotation_x(xReloc.roll_calc, Eigen::Vector3f::UnitX());
+        Eigen::AngleAxisf init_rotation_y(xReloc.pitch_calc, Eigen::Vector3f::UnitY());
+        Eigen::AngleAxisf init_rotation_z(xReloc.yaw_calc, Eigen::Vector3f::UnitZ());
+        Eigen::Matrix4f init_guess = (init_translation * init_rotation_z * init_rotation_y * init_rotation_x) * tf_btol;
+
+        pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
+        pndt->align(*aligned,init_guess);
+
+        bool has_converged;
+        int iteration = 0;
+        double fitness_score = 0.0;
+        double trans_probability = 0.0;
+        Eigen::Matrix4f t(Eigen::Matrix4f::Identity());
+        Eigen::Matrix4f t2(Eigen::Matrix4f::Identity());
+
+
+        has_converged = pndt->hasConverged();
+        t = pndt->getFinalTransformation();
+        iteration = pndt->getFinalNumIteration();
+        fitness_score = pndt->getFitnessScore();
+        trans_probability = pndt->getTransformationProbability();
+
+        t2 = t * tf_btol.inverse();
+
+        double x,y,z,yaw,pitch,roll;
+
+        tf::Matrix3x3 mat_b;  // base_link
+        mat_b.setValue(static_cast<double>(t2(0, 0)), static_cast<double>(t2(0, 1)), static_cast<double>(t2(0, 2)),
+                       static_cast<double>(t2(1, 0)), static_cast<double>(t2(1, 1)), static_cast<double>(t2(1, 2)),
+                       static_cast<double>(t2(2, 0)), static_cast<double>(t2(2, 1)), static_cast<double>(t2(2, 2)));
+
+        // Update ndt_pose
+        x = t2(0, 3);
+        y = t2(1, 3);
+        z = t2(2, 3);
+
+        //      ndt_pose.x = localizer_pose.x;
+        //      ndt_pose.y = localizer_pose.y;
+        //      ndt_pose.z = localizer_pose.z;
+        mat_b.getRPY(roll, pitch, yaw, 1);
+
+        xReloc.x_calc = x;
+        xReloc.y_calc = y;
+        xReloc.z_calc = z;
+        xReloc.roll_calc = roll;
+        xReloc.pitch_calc = pitch;
+        xReloc.yaw_calc =yaw;
+        xReloc.trans_prob = trans_probability;
+
+
+ //       std::cout<<"guass x:"<<xReloc.x_guess<<" res:"<<" x: "<<xReloc.x_calc<<" y: "<<xReloc.y_calc<<" prob: "<<xReloc.trans_prob<<" step: "<<i<<std::endl;
+
+        if((i>=10)&&(xReloc.trans_prob<1.0))
+        {
+            break;
+        }
+
+        if((xReloc.trans_prob>3.0) && ((xReloc.trans_prob - fLastProb)<0.01) && (i>20))
+        {
+            break;
+        }
+
+        if((xReloc.trans_prob<1.5) && ((xReloc.trans_prob - fLastProb)<0.01) && (i>20))
+        {
+            break;
+        }
+
+        fLastProb = xReloc.trans_prob;
+
+    }
+
+    mmutexReloc.lock();
+    mvectorReloc.push_back(xReloc);
+    mmutexReloc.unlock();
+
+    std::cout<<"guass x:"<<xReloc.x_guess<<" y:"<<xReloc.y_guess<<" res:"<<" x: "<<xReloc.x_calc<<" y: "<<xReloc.y_calc<<" prob: "<<xReloc.trans_prob<<std::endl;
+
+    mthreadcomplete[nThread] = true;
+
+}
+
+void GlobalRelocation::RunReloc(std::vector<iv::ndttracepoint> & xtrace,pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan,bool bCheckComplete)
+{
+    int i;
+    bool bComplete = false;
+    bool bHaveTask = true;
+    int ntracesize = static_cast<int>(xtrace.size());
+    int j = 0;
+    if(j == ntracesize)bHaveTask = false;
+    while(bComplete == false)
+    {
+        if(bHaveTask)
+        {
+            for(i=0;i<mnThreadNum;i++)
+            {
+                if((mthreadcomplete[i] == true) &&(bHaveTask))
+                {
+                    if(mpthread[i] != NULL)
+                    {
+                        mpthread[i]->join();
+                        delete mpthread[i];
+                        mpthread[i] = NULL;
+                    }
+                    iv::Reloc xReloc;
+                    xReloc.x_guess = xtrace[j].x;
+                    xReloc.y_guess = xtrace[j].y;
+                    xReloc.z_guess = xtrace[j].z;
+                    xReloc.yaw_guess = xtrace[j].yaw;
+                    xReloc.pitch_guess = 0;
+                    xReloc.roll_guess = 0;
+                    if(j == ntracesize)bHaveTask = false;
+    //                std::thread * pthread = new std::thread(&GlobalRelocation::threadtest,this,i,xReloc);
+                    mthreadcomplete[i] = false;
+                    mpthread[i] = new std::thread(&GlobalRelocation::threadReloc,this,i,&mndt[i],raw_scan,xReloc);
+
+                    j++;
+                    if(j == ntracesize)bHaveTask = false;
+                }
+            }
+        }
+        else
+        {
+            for(i=0;i<mnThreadNum;i++)
+            {
+                if((mthreadcomplete[i] == true))
+                {
+                    if(mpthread[i] != NULL)
+                    {
+                        mpthread[i]->join();
+                        delete mpthread[i];
+                        mpthread[i] = NULL;
+                    }
+                }
+                else
+                {
+                    break;
+                }
+            }
+            if(i == mnThreadNum)
+            {
+                bComplete = true;
+                break;
+            }
+        }
+
+        if(bCheckComplete)
+        {
+            for(i=0;i<mvectorReloc.size();i++)
+            {
+                if(mvectorReloc[i].trans_prob >= 3.0)
+                {
+                    bHaveTask = false;
+                    std::cout<<" Complete. "<<std::endl;
+                }
+            }
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+
+}
+
+
+iv::Reloc GlobalRelocation::pointreloc(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
+{
+    iv::Reloc xreloczero;
+    xreloczero.trans_prob = 0.0;
+
+    if(mvectorndtmap.size() > 1)
+    {
+        std::cout<<" only one map, can use relocation."<<std::endl;
+
+        return xreloczero;
+    }
+
+    if(mvectorndtmap.size() == 0)
+    {
+        std::cout<<" no map."<<std::endl;
+        return xreloczero;
+    }
+
+    mvectorReloc.clear();
+
+    int i;
+    int j;
+    for(i=0;i<mnThreadNum;i++)
+    {
+        mthreadcomplete[i] = true;
+    }
+
+    std::vector<iv::ndttracepoint> xtrace = mvectorseedpoint;
+
+    RunReloc(xtrace,raw_scan,false);
+
+    int nrelocsize = mvectorReloc.size();
+    int indexmax = 0;
+    double fMaxProb = 0.0;
+    for(i=0;i<nrelocsize;i++)
+    {
+        if(mvectorReloc[i].trans_prob>fMaxProb)
+        {
+            fMaxProb  = mvectorReloc[i].trans_prob;
+            indexmax = i;
+        }
+    }
+
+    if(fMaxProb > 2.0)
+    {
+
+        return mvectorReloc[indexmax];
+    }
+
+    std::cout<<" use diff yaw"<<std::endl;
+
+    std::vector<iv::ndttracepoint> xtraceraw = mvectorseedpoint;
+    xtrace.clear();
+
+    for(i=0;i<xtraceraw.size();i++)
+    {
+        iv::ndttracepoint ntp = xtraceraw[i];
+
+        for(j=1;j<mnCirclDivide;j++)
+        {
+            double yawadd = M_PI*2.0/mnCirclDivide;
+            yawadd = yawadd * j;
+            iv::ndttracepoint ntpyaw = ntp;
+            ntpyaw.yaw = ntp.yaw + yawadd;
+            if(ntpyaw.yaw >= 2.0*M_PI)ntpyaw.yaw = ntpyaw.yaw - 2.0*M_PI;
+            xtrace.push_back(ntpyaw);
+        }
+    }
+
+
+    RunReloc(xtrace,raw_scan,false);
+
+    nrelocsize = mvectorReloc.size();
+    indexmax = 0;
+    fMaxProb = 0.0;
+    for(i=0;i<nrelocsize;i++)
+    {
+        if(mvectorReloc[i].trans_prob>fMaxProb)
+        {
+            fMaxProb  = mvectorReloc[i].trans_prob;
+            indexmax = i;
+        }
+    }
+
+    if(fMaxProb > 2.0)
+    {
+        return mvectorReloc[indexmax];
+    }
+
+    std::cout<<" use dis yaw"<<std::endl;
+
+    xtraceraw = mvectorseedpoint;
+    xtrace.clear();
+
+    int k;
+    for(k=1;k<10;k++)
+    {
+    for(i=0;i<xtraceraw.size();i++)
+    {
+        iv::ndttracepoint ntpraw = xtraceraw[i];
+        iv::ndttracepoint ntp = ntpraw;
+        ntp.x = ntpraw.x + k*mfSeedDisThresh * cos(ntpraw.yaw + M_PI/2.0);
+        ntp.y = ntpraw.y + k*mfSeedDisThresh * sin(ntpraw.yaw + M_PI/2.0);
+        for(j=0;j<mnCirclDivide;j++)
+        {
+            double yawadd = M_PI*2.0/mnCirclDivide;
+            yawadd = yawadd * j;
+            iv::ndttracepoint ntpyaw = ntp;
+            ntpyaw.yaw = ntp.yaw + yawadd;
+            if(ntpyaw.yaw >= 2.0*M_PI)ntpyaw.yaw = ntpyaw.yaw - 2.0*M_PI;
+            xtrace.push_back(ntpyaw);
+        }
+        ntp = ntpraw;
+        ntp.x = ntpraw.x + k*mfSeedDisThresh * cos(ntpraw.yaw - M_PI/2.0);
+        ntp.y = ntpraw.y + k*mfSeedDisThresh * sin(ntpraw.yaw - M_PI/2.0);
+        for(j=0;j<mnCirclDivide;j++)
+        {
+            double yawadd = M_PI*2.0/mnCirclDivide;
+            yawadd = yawadd * j;
+            iv::ndttracepoint ntpyaw = ntp;
+            ntpyaw.yaw = ntp.yaw + yawadd;
+            if(ntpyaw.yaw >= 2.0*M_PI)ntpyaw.yaw = ntpyaw.yaw - 2.0*M_PI;
+            xtrace.push_back(ntpyaw);
+        }
+    }
+    }
+
+
+    RunReloc(xtrace,raw_scan,true);
+
+    nrelocsize = mvectorReloc.size();
+    indexmax = 0;
+    fMaxProb = 0.0;
+    for(i=0;i<nrelocsize;i++)
+    {
+        if(mvectorReloc[i].trans_prob>fMaxProb)
+        {
+            fMaxProb  = mvectorReloc[i].trans_prob;
+            indexmax = i;
+        }
+    }
+
+    if(fMaxProb > 2.0)
+    {
+        std::cout<<" find pos."<<std::endl;
+        return mvectorReloc[indexmax];
+    }
+
+
+    if(mvectorReloc.size() > 0)
+    {
+        return mvectorReloc[indexmax];
+    }
+
+
+    return xreloczero;
+
+
+}
+
+void GlobalRelocation::setndtmap(std::vector<iv::ndtmaptrace> & xvectorndtmap)
+{
+    mvectorndtmap = xvectorndtmap;
+    if(mvectorndtmap.size() == 0)
+    {
+        return;
+    }
+    int i;
+    for(i=0;i<mnThreadNum;i++)
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr xmap;
+
+        xmap = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);
+
+
+        QTime xTime;
+        xTime.start();
+        if(0 == pcl::io::loadPCDFile(mvectorndtmap[0].mstrpcdpath.data(),*xmap))
+        {
+
+        }
+        else
+        {
+            std::cout<<"map size is 0"<<std::endl;
+            return;
+        }
+        mbRelocAvail = true;
+        mndt[i].setInputTarget(xmap);
+        mthreadcomplete[i] = true;
+    }
+
+
+    int npointsize = mvectorndtmap[0].mvector_trace.size();
+    if(npointsize == 0)
+    {
+        return;
+    }
+    mvectorseedpoint.clear();
+    std::cout<<" load trace seed."<<std::endl;
+    iv::ndttracepoint ntp = mvectorndtmap[0].mvector_trace[0];
+    mvectorseedpoint.push_back(ntp);
+
+    for(i=1;i<npointsize;i++)
+    {
+        iv::ndttracepoint ntpnow = mvectorndtmap[0].mvector_trace[i];
+        iv::ndttracepoint ntplast = mvectorseedpoint[mvectorseedpoint.size() -1];
+        double fdis = sqrt(pow(ntpnow.x - ntplast.x,2) + pow(ntpnow.y - ntplast.y,2));
+        double fyawdiff = ntpnow.yaw - ntplast.yaw;
+        while(fyawdiff<(-M_PI))fyawdiff = fyawdiff + 2.0*M_PI;
+        while(fyawdiff >(M_PI))fyawdiff = fyawdiff - 2.0*M_PI;
+        if(fdis>=mfSeedDisThresh)
+        {
+            mvectorseedpoint.push_back(ntpnow);
+        }
+        else
+        {
+            if(fabs(fyawdiff)>0.1)
+            {
+                mvectorseedpoint.push_back(ntpnow);
+            }
+            else
+            {
+                if(i== (npointsize -1))
+                {
+                    mvectorseedpoint.push_back(ntpnow);
+                }
+            }
+        }
+    }
+
+    if(mvectorseedpoint.size()>0)
+    {
+        const int nfrontadd = 3;
+        const int nrearadd  = 3;
+        iv::ndttracepoint ntpfirst = mvectorseedpoint[0];
+        iv::ndttracepoint ntplast = mvectorseedpoint[mvectorseedpoint.size() -1];
+        for(i=0;i<nfrontadd;i++)
+        {
+            iv::ndttracepoint ntpnow = ntpfirst;
+            double foff = (i+1)*mfSeedDisThresh;
+            ntpnow.x = ntpfirst.x + foff*cos(ntpfirst.yaw + M_PI);
+            ntpnow.y = ntpfirst.y + foff*sin(ntpfirst.yaw + M_PI);
+            mvectorseedpoint.insert(mvectorseedpoint.begin(),ntpnow);
+        }
+        for(i=0;i<nrearadd;i++)
+        {
+            iv::ndttracepoint ntpnow = ntplast;
+            double foff = (i+1)*mfSeedDisThresh;
+            ntpnow.x = ntplast.x + foff*cos(ntpfirst.yaw);
+            ntpnow.y = ntplast.y + foff*sin(ntpfirst.yaw );
+            mvectorseedpoint.push_back(ntpnow);
+        }
+    }
+
+//        for(i=0;i<mvectorseedpoint.size();i++)
+//        {
+//            mvectorseedpoint[i].yaw = mvectorseedpoint[i].yaw + M_PI;
+//            mvectorseedpoint[i].y = mvectorseedpoint[i].y + 10.0;
+//        }
+
+
+}

src/detection/detection_ndt_matching/globalrelocation.h

@@ -0,0 +1,76 @@
+#ifndef GLOBALRELOCATION_H
+#define GLOBALRELOCATION_H
+
+#include <ndt_cpu/NormalDistributionsTransform.h>
+
+#include <thread>
+#include <mutex>
+#include "ndt_matching.h"
+
+#define MAX_NDT 100
+
+
+namespace  iv {
+struct Reloc
+{
+    double x_guess;
+    double y_guess;
+    double z_guess;
+    double yaw_guess;
+    double pitch_guess;
+    double roll_guess;
+    double x_calc;
+    double y_calc;
+    double z_calc;
+    double yaw_calc;
+    double pitch_calc;
+    double roll_calc;
+    double trans_prob;
+};
+}
+
+class GlobalRelocation
+{
+public:
+    GlobalRelocation();
+
+    iv::Reloc pointreloc(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan);
+
+    void setndtmap(std::vector<iv::ndtmaptrace> & xvectorndtmap);
+
+private:
+    cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> mndt[MAX_NDT];
+
+    std::vector<iv::ndtmaptrace> mvectorndtmap;
+
+    std::vector<iv::Reloc> mvectorReloc;
+    std::mutex mmutexReloc;
+
+    std::vector<iv::ndttracepoint> mvectorseedpoint;
+
+    bool mbRelocAvail = false;
+
+
+
+    bool mthreadcomplete[MAX_NDT];
+
+
+
+    void threadtest(int n,iv::Reloc & xReloc);
+
+    void threadReloc(int nThread,cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> * pndt,
+                     pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan,
+                     iv::Reloc xReloc);
+
+    void RunReloc(std::vector<iv::ndttracepoint> & xtrace,pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan,bool bCheckComplete);
+
+    int mnThreadNum = 8;
+
+    std::thread * mpthread[MAX_NDT];
+
+    const double mfSeedDisThresh = 3.0;
+    const int mnCirclDivide  = 20;
+
+};
+
+#endif // GLOBALRELOCATION_H

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,665 @@
+#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"
+
+#include "globalrelocation.h"
+
+extern bool gbuseanh;
+
+
+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
+
+GlobalRelocation gGlobalRelocation;
+
+bool gbNeedReloc = false;
+
+bool gbEnableReloc = true; //if have rtk position, is false;
+
+/**
+ * @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 int LoadLastPos(const char * strlastposfilepath)
+{
+    int nres = -1;
+    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.");
+        }
+        else
+        {
+            nres = 1;
+        }
+        if(gbSaveLastPos)gpFileLastPos = xFile;
+        else
+        {
+            gpFileLastPos = 0;
+            xFile->close();
+        }
+    }
+
+    return nres;
+}
+
+/**
+ * @brief LoadTrace
+ */
+static void LoadTrace()
+{
+    std::string strpath = getenv("HOME");
+    strpath = strpath + "/map/";
+
+    std::string strlastposfile = strpath;
+    strlastposfile = strlastposfile + "lastpos.txt";
+    int nload = 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.");
+        }
+    }
+
+    if(nload != 1)
+    {
+        std::cout<<" no lastpos.txt file."<<std::endl;
+        if(gvector_trace.size() == 1)
+        {
+            std::cout<<" only one .pcd. so use .ori pos as start pos."<<std::endl;
+            glastndtgpspos = gvector_trace[0].mndtorigin;
+            std::cout<<" use ori pos as lastpos."<<std::endl;
+        }
+
+    }
+
+    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());
+
+
+    if(gbNeedReloc && gbEnableReloc)
+    {
+        static int nRelocFail = 0;
+        iv::Reloc xreloc = gGlobalRelocation.pointreloc(point_cloud);
+        restartndtfailcount();
+        if(xreloc.trans_prob>=2.0)
+        {
+            pose posereloc;
+            posereloc.vx = 0;
+            posereloc.vy = 0;
+            posereloc.x = xreloc.x_calc;
+            posereloc.y = xreloc.y_calc;
+            posereloc.z = xreloc.z_calc;
+            posereloc.yaw = xreloc.yaw_calc;
+            posereloc.pitch = xreloc.pitch_calc;
+            posereloc.roll = xreloc.roll_calc;
+            setrelocpose(posereloc);
+        }
+        else
+        {
+            std::cout<<" reloc fail."<<std::endl;
+            nRelocFail++;
+        }
+        if(nRelocFail >= 3)
+        {
+            std::cout<<" more than 3 reloc fail. disable reloc."<<std::endl;
+            gbEnableReloc = false;
+        }
+        gbNeedReloc = false;
+    }
+ //
+
+    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++;
+        gbEnableReloc = false;
+    }
+    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;
+}
+
+void threadrelocation()
+{
+    while(gbstate)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    showversion("detection_ndt_matching");
+    RegisterIVBackTrace();
+    QCoreApplication a(argc, argv);
+
+    QString strpath = QCoreApplication::applicationDirPath();
+
+   if(argc < 2)
+       strpath = strpath + "/detection_ndt_matching.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");
+
+   gbuseanh = xparam.GetParam("useanh",false);
+
+   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;
+
+    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);
+    gpd = iv::modulecomm::RegisterSend(gstr_ndtgpsposmsgname.data(),10000,1);
+    ndt_match_Init_nomap();
+
+    iv::ivexit::RegIVExitCall(exitfunc);
+
+
+    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,1936 @@
+/*
+ * 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_cpu/NormalDistributionsTransform.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;
+
+extern bool gbNeedReloc;
+
+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;
+
+bool gbuseanh = true;
+
+static pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
+
+static cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> anh_ndt;
+static cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> glocalanh_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 ;
+
+static int gndtcalcfailcount = 0;
+static pose gPoseReloc;
+static bool gbPoseReloc = false;
+
+#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*180.0/M_PI  + xori.pitch;
+    xgpspos.roll  = xpose.roll*180.0/M_PI + 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);
+
+
+//    std::cout<< " ori heading :  "<<xori.heading<<std::endl;
+
+//    std::cout<<" pose yaw: "<<xpose.yaw<<std::endl;
+
+    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)*M_PI/180.0;
+    posex.roll = (xcur.roll - xori.roll)*M_PI/180.0;
+    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;
+
+
+    if(gbuseanh)
+    {
+        cpu::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> localanhraw;
+
+        localanhraw.setResolution(ndt_res);
+        localanhraw.setInputTarget(map_ptr);
+        localanhraw.setMaximumIterations(max_iter);
+        localanhraw.setStepSize(step_size);
+        localanhraw.setTransformationEpsilon(trans_eps);
+
+        pthread_mutex_lock(&mutex);
+        //        ndt = glocal_ndt;
+        glocalanh_ndt = localanhraw;
+        pthread_mutex_unlock(&mutex);
+
+        gbNeedUpdate = true;
+    }
+    else
+    {
+
+        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);
+                std::cout<<"gps fix ndt pos "<<" dis: "<<dis<<" headdiff: "<<headdiff<<" zdiff: "<<zdiff<<std::endl;
+//                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;
+}
+
+
+void restartndtfailcount()
+{
+    gndtcalcfailcount = 0;
+}
+
+void setrelocpose(pose xPose)
+{
+    gPoseReloc = xPose;
+    gbPoseReloc = true;
+}
+
+
+#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;
+//        gcurndtgpspos.pitch = 0; //not use gps pitch and roll
+//        gcurndtgpspos.roll = 0;
+        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)
+    {
+        if(gbuseanh)
+        {
+            pthread_mutex_lock(&mutex);
+            anh_ndt = glocalanh_ndt;
+            pthread_mutex_unlock(&mutex);
+        }
+        else
+        {
+            pthread_mutex_lock(&mutex);
+            ndtraw = glocalndtraw;
+            pthread_mutex_unlock(&mutex);
+        }
+
+        gusemapindex = gcurmapindex;
+        gbNeedUpdate = false;
+
+    }
+
+
+
+
+
+    previous_pose = CalcPose(gvector_trace[gusemapindex].mndtorigin,gcurndtgpspos);
+
+    if(gbPoseReloc)
+    {
+        previous_pose = gPoseReloc;
+        gbPoseReloc = false;
+    }
+
+ //   std::cout<<" previos head: "<<previous_pose.yaw<<std::endl;
+    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);
+
+
+    if(gbuseanh)
+    {
+#ifdef  TEST_CALCTIME
+        std::cout<<"use anh."<<std::endl;
+#endif
+        anh_ndt.setInputSource(filtered_scan_ptr);
+    }
+    else
+    {
+#ifdef  TEST_CALCTIME
+        std::cout<<"use ndt."<<std::endl;
+#endif
+        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;
+
+
+
+//      predict_pose_for_ndt.pitch = 0;
+//      predict_pose_for_ndt.roll = 0;
+      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);
+//      qDebug("pre x:%f y:%f z:%f yaw:%f pitch: %f roll: %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.pitch,predict_pose_for_ndt.roll);
+//      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;
+
+
+  //    std::cout<<" gues pose yaw: "<<predict_pose_for_ndt.yaw<<std::endl;
+      pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
+      align_start = std::chrono::system_clock::now();
+      if(gbuseanh)
+      {
+        anh_ndt.align(*aligned,init_guess);
+      }
+      else
+        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
+
+      if(gbuseanh)
+      {
+          has_converged = anh_ndt.hasConverged();
+          t = anh_ndt.getFinalTransformation();
+          iteration = anh_ndt.getFinalNumIteration();
+          getFitnessScore_start = std::chrono::system_clock::now();
+          fitness_score = anh_ndt.getFitnessScore();
+          getFitnessScore_end = std::chrono::system_clock::now();
+          trans_probability = anh_ndt.getTransformationProbability();
+      }
+      else
+      {
+          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();
+      }
+
+
+      if(trans_probability >= 1.0)
+      {
+          gndtcalcfailcount = 0;
+      }
+      else
+      {
+          gndtcalcfailcount++;
+      }
+
+      if(gndtcalcfailcount >= 30)
+      {
+          gbNeedReloc = true;
+      }
+
+
+      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,116 @@
+#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);
+
+void restartndtfailcount();
+
+void setrelocpose(pose xPose);
+
+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_matching_gpu_multi/detection_ndt_matching_gpu_multi.pro

@@ -43,12 +43,15 @@ 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/
 
 INCLUDEPATH += /usr/local/cuda-10.0/targets/aarch64-linux/include
 INCLUDEPATH += /usr/local/cuda-10.2/targets/aarch64-linux/include
 INCLUDEPATH += /usr/local/cuda-10.2/targets/x86_64-linux/include
+INCLUDEPATH += /usr/local/cuda/targets/aarch64-linux/include
 
 DEFINES += CUDA_FOUND
 

src/detection/detection_ndt_matching_gpu_multi/main.cpp

@@ -37,6 +37,7 @@ 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.
 
@@ -430,6 +431,8 @@ void ListenRaw(const char * strdata,const unsigned int nSize,const unsigned int
     if(nFixCount < 300)gbGPSFix = false;
     else gbGPSFix = true;
 
+    gnLastGPSTime = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
 }
 
 
@@ -514,7 +517,7 @@ int main(int argc, char *argv[])
    iv::xmlparam::Xmlparam xparam(strpath.toStdString());
 
    gstr_lidarmsgname = xparam.GetParam("lidarmsg","lidar_pc");
-   gstr_gpsmsgname = xparam.GetParam("gpsmsg","ins550d_gpsimu");
+   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");

src/detection/detection_ndt_matching_gpu_multi/ndt_matching.cpp

@@ -297,6 +297,7 @@ extern iv::gpspos glastndtgpspos;
 extern iv::gpspos gcurndtgpspos;
 extern iv::gpspos gcurgpsgpspos;
 extern bool gbGPSFix;
+extern int64_t gnLastGPSTime;
 
 static bool gbgpsupdatendt = false;
 
@@ -327,8 +328,8 @@ iv::gpspos PoseToGPS(iv::gpspos xori,pose xpose)
     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.pitch = xpose.pitch *180.0/M_PI + xori.pitch;
+    xgpspos.roll  = xpose.roll *180.0/M_PI + 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);
 
@@ -382,8 +383,8 @@ pose CalcPose(iv::gpspos xori, iv::gpspos xcur)
     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.pitch = (xcur.pitch - xori.pitch)*M_PI/180.0;
+    posex.roll = (xcur.roll - xori.roll)*M_PI/180.0;
     posex.yaw = rel_hdg;
 
     posex.vx = xcur.ve * cos(-hdg_o) - xcur.vn * sin(-hdg_o);
@@ -449,6 +450,7 @@ static void getmindistrace(int & index, double & ftracedis)
     else
     {
         index = -1;
+        ftracedis = 100000.0;
     }
 }
 
@@ -530,7 +532,7 @@ void LocalMapUpdate(int n)
     {
 
         getmindistrace(index,ftracedis);
-//        std::cout<<"trace dis is "<<ftracedis<<std::endl;
+
         if(g_hasmap == false)
         {
 
@@ -561,10 +563,12 @@ void LocalMapUpdate(int n)
 
         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));
     }
 }
@@ -607,6 +611,12 @@ void GPSPosMonitor(bool * pbRun)
     }
     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;
@@ -1217,6 +1227,7 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
     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;
@@ -1345,7 +1356,7 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
     old_scan_time = current_scan_time;
 
     if(diff_time<=0)diff_time = 0.1;
-    if(diff_time>1.0)diff_time = 1.0;
+    if(diff_time>1.0)diff_time = 0.1;
 
 //    std::cout<<"diff time is "<<diff_time<<std::endl;
 
@@ -1404,6 +1415,8 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
           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;

src/detection/detection_ndt_pclomp/detection_ndt_matching_gpu_multi.xml

@@ -0,0 +1,13 @@
+<xml>	
+	<node name="detection_ndt_matching_gpu_multi.xml">
+		<param name="lidarmsg" value="lidarpc_center" />
+		<param name="gpsmsg" value="ins550d_gpsimu" />
+		<param name="ndtposmsg" value="ndtpos" />
+		<param name="ndtgpsposmsg" value="ndtgpspos" />
+		<param name="AngleCorrect" value="0.0" />
+		<param name="HeightCorrect" value="0.0" />
+		<param name="SaveLastPos" value="true" />
+		<param name="Arm_LidarGPSBase_x" value="0.9" />
+		<param name="Arm_LidarGPSBase_y" value="0.0" />
+	</node>
+</xml>

src/detection/detection_ndt_pclomp/detection_ndt_pclomp.pro

@@ -0,0 +1,97 @@
+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 \
+    pclomp/ndt_omp.cpp \
+    pclomp/voxel_grid_covariance_omp.cpp
+
+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 \
+    pclomp/ndt_omp_impl.hpp \
+    pclomp/voxel_grid_covariance_omp_impl.hpp \
+    pclomp/pclomp/ndt_omp.h \
+    pclomp/pclomp/voxel_grid_covariance_omp.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/
+
+INCLUDEPATH += $$PWD/pclomp
+
+DEFINES += _OPENMP
+#DEFINES += USE_PCL_OPENMP
+
+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
+
+
+#INCLUDEPATH += $$PWD/../../common/ndt_gpu/
+
+
+!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
+
+LIBS += -lmpi -lopen-pal -lgomp
+

src/detection/detection_ndt_pclomp/detection_ndt_pclomp.xml

@@ -0,0 +1,13 @@
+<xml>	
+	<node name="detection_ndt_pclomp.xml">
+		<param name="lidarmsg" value="lidarpc_center" />
+		<param name="gpsmsg" value="ins550d_gpsimu" />
+		<param name="ndtposmsg" value="ndtpos" />
+		<param name="ndtgpsposmsg" value="ndtgpspos" />
+		<param name="AngleCorrect" value="0.0" />
+		<param name="HeightCorrect" value="0.0" />
+		<param name="SaveLastPos" value="true" />
+		<param name="Arm_LidarGPSBase_x" value="0.0" />
+		<param name="Arm_LidarGPSBase_y" value="0.0" />
+	</node>
+</xml>

src/detection/detection_ndt_pclomp/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_pclomp/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_pclomp/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_pclomp");
+    QCoreApplication a(argc, argv);
+
+    QString strpath = QCoreApplication::applicationDirPath();
+
+   if(argc < 2)
+       strpath = strpath + "/detection_ndt_pclomp.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_pclomp/ndt_matching.cpp

@@ -0,0 +1,1849 @@
+/*
+ * 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 <pclomp/ndt_omp.h>
+
+#include <QFile>
+
+//#include <ndt_gpu/NormalDistributionsTransform.h>
+#include "ndtpos.pb.h"
+#include "ndtgpspos.pb.h"
+
+#ifdef USE_PCL_OPENMP
+#include <pcl_omp_registration/ndt.h>
+#endif
+
+#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;
+
+
+pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+
+pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr glocalndt_omp(new pclomp::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;
+
+    std::cout<<" max threads. "<<omp_get_max_threads()<<std::endl;
+
+    int nthread = omp_get_max_threads();
+    nthread = nthread - 2;
+    if(nthread <1)nthread = 1;
+
+    std::cout<<" use threads: "<<nthread<<std::endl;
+
+    pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr localndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+
+
+
+    localndt_omp->setNumThreads(nthread);
+    localndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7);
+
+
+    localndt_omp->setResolution(ndt_res);
+    localndt_omp->setInputTarget(map_ptr);
+    localndt_omp->setMaximumIterations(max_iter);
+    localndt_omp->setStepSize(step_size);
+    localndt_omp->setTransformationEpsilon(trans_eps);
+
+    pthread_mutex_lock(&mutex);
+//        ndt = glocal_ndt;
+    glocalndt_omp = localndt_omp;
+    pthread_mutex_unlock(&mutex);
+
+    gbNeedUpdate = true;
+
+
+    std::cout<<" ndt_omp init successfully. "<<std::endl;
+
+
+    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);
+
+
+    ndt_omp->setResolution(ndt_res);
+    ndt_omp->setInputTarget(map_ptr);
+    ndt_omp->setMaximumIterations(max_iter);
+    ndt_omp->setStepSize(step_size);
+    ndt_omp->setTransformationEpsilon(trans_eps);
+
+    ndt_omp->setNumThreads(6);
+    ndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7);
+
+    std::cout<<" ndt_omp init success. "<<std::endl;
+
+
+
+    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);
+        ndt_omp = glocalndt_omp;
+        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);
+
+
+    ndt_omp->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();
+      ndt_omp->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 = ndt_omp->hasConverged();
+
+      t = ndt_omp->getFinalTransformation();
+      iteration = ndt_omp->getFinalNumIteration();
+
+      getFitnessScore_start = std::chrono::system_clock::now();
+      fitness_score = ndt_omp->getFitnessScore();
+      getFitnessScore_end = std::chrono::system_clock::now();
+
+      trans_probability = ndt_omp->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_pclomp/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/pclomp/ndt_omp.cpp

@@ -0,0 +1,5 @@
+#include <pclomp/ndt_omp.h>
+#include <ndt_omp_impl.hpp>
+
+template class pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>;
+template class pclomp::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>;

src/detection/detection_ndt_pclomp/pclomp/ndt_omp_impl.hpp

@@ -0,0 +1,985 @@
+#include <pclomp/ndt_omp.h>
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-2011, Willow Garage, Inc.
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_REGISTRATION_NDT_OMP_IMPL_H_
+#define PCL_REGISTRATION_NDT_OMP_IMPL_H_
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget>
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::NormalDistributionsTransform ()
+  : target_cells_ ()
+  , resolution_ (1.0f)
+  , step_size_ (0.1)
+  , outlier_ratio_ (0.55)
+  , gauss_d1_ ()
+  , gauss_d2_ ()
+  , gauss_d3_ ()
+  , trans_probability_ ()
+  , j_ang_a_ (), j_ang_b_ (), j_ang_c_ (), j_ang_d_ (), j_ang_e_ (), j_ang_f_ (), j_ang_g_ (), j_ang_h_ ()
+  , h_ang_a2_ (), h_ang_a3_ (), h_ang_b2_ (), h_ang_b3_ (), h_ang_c2_ (), h_ang_c3_ (), h_ang_d1_ (), h_ang_d2_ ()
+  , h_ang_d3_ (), h_ang_e1_ (), h_ang_e2_ (), h_ang_e3_ (), h_ang_f1_ (), h_ang_f2_ (), h_ang_f3_ ()
+{
+  reg_name_ = "NormalDistributionsTransform";
+
+  double gauss_c1, gauss_c2;
+
+  // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009]
+  gauss_c1 = 10.0 * (1 - outlier_ratio_);
+  gauss_c2 = outlier_ratio_ / pow (resolution_, 3);
+  gauss_d3_ = -log (gauss_c2);
+  gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3_;
+  gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3_) / gauss_d1_);
+
+  transformation_epsilon_ = 0.1;
+  max_iterations_ = 35;
+
+  search_method = DIRECT7;
+  num_threads_ = omp_get_max_threads();
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
+{
+  nr_iterations_ = 0;
+  converged_ = false;
+
+  double gauss_c1, gauss_c2;
+
+  // Initializes the gaussian fitting parameters (eq. 6.8) [Magnusson 2009]
+  gauss_c1 = 10 * (1 - outlier_ratio_);
+  gauss_c2 = outlier_ratio_ / pow (resolution_, 3);
+  gauss_d3_ = -log (gauss_c2);
+  gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3_;
+  gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3_) / gauss_d1_);
+
+  if (guess != Eigen::Matrix4f::Identity ())
+  {
+    // Initialise final transformation to the guessed one
+    final_transformation_ = guess;
+    // Apply guessed transformation prior to search for neighbours
+    transformPointCloud (output, output, guess);
+  }
+
+  Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
+  eig_transformation.matrix () = final_transformation_;
+
+  // Convert initial guess matrix to 6 element transformation vector
+  Eigen::Matrix<double, 6, 1> p, delta_p, score_gradient;
+  Eigen::Vector3f init_translation = eig_transformation.translation ();
+  Eigen::Vector3f init_rotation = eig_transformation.rotation ().eulerAngles (0, 1, 2);
+  p << init_translation (0), init_translation (1), init_translation (2),
+  init_rotation (0), init_rotation (1), init_rotation (2);
+
+  Eigen::Matrix<double, 6, 6> hessian;
+
+  double score = 0;
+  double delta_p_norm;
+
+  // Calculate derivatives of initial transform vector, subsequent derivative calculations are done in the step length determination.
+  score = computeDerivatives (score_gradient, hessian, output, p);
+
+  while (!converged_)
+  {
+    // Store previous transformation
+    previous_transformation_ = transformation_;
+
+    // Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson 2009]
+    Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6> > sv (hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
+    // Negative for maximization as opposed to minimization
+    delta_p = sv.solve (-score_gradient);
+
+    //Calculate step length with guaranteed sufficient decrease [More, Thuente 1994]
+    delta_p_norm = delta_p.norm ();
+
+    if (delta_p_norm == 0 || delta_p_norm != delta_p_norm)
+    {
+      trans_probability_ = score / static_cast<double> (input_->points.size ());
+      converged_ = delta_p_norm == delta_p_norm;
+      return;
+    }
+
+    delta_p.normalize ();
+    delta_p_norm = computeStepLengthMT (p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, output);
+    delta_p *= delta_p_norm;
+
+
+    transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (delta_p (0)), static_cast<float> (delta_p (1)), static_cast<float> (delta_p (2))) *
+                       Eigen::AngleAxis<float> (static_cast<float> (delta_p (3)), Eigen::Vector3f::UnitX ()) *
+                       Eigen::AngleAxis<float> (static_cast<float> (delta_p (4)), Eigen::Vector3f::UnitY ()) *
+                       Eigen::AngleAxis<float> (static_cast<float> (delta_p (5)), Eigen::Vector3f::UnitZ ())).matrix ();
+
+
+    p = p + delta_p;
+
+    // Update Visualizer (untested)
+    if (update_visualizer_ != 0)
+      update_visualizer_ (output, std::vector<int>(), *target_, std::vector<int>() );
+
+    if (nr_iterations_ > max_iterations_ ||
+        (nr_iterations_ && (std::fabs (delta_p_norm) < transformation_epsilon_)))
+    {
+      converged_ = true;
+    }
+
+    nr_iterations_++;
+
+  }
+
+  // Store transformation probability. The relative differences within each scan registration are accurate
+  // but the normalization constants need to be modified for it to be globally accurate
+  trans_probability_ = score / static_cast<double> (input_->points.size ());
+}
+
+#ifndef _OPENMP
+int omp_get_max_threads() { return 1; }
+int omp_get_thread_num() { return 0; }
+#endif
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> double
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient,
+	Eigen::Matrix<double, 6, 6> &hessian,
+	PointCloudSource &trans_cloud,
+	Eigen::Matrix<double, 6, 1> &p,
+	bool compute_hessian)
+{
+	score_gradient.setZero();
+	hessian.setZero();
+	double score = 0;
+
+  std::vector<double> scores(input_->points.size());
+  std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1>>> score_gradients(input_->points.size());
+  std::vector<Eigen::Matrix<double, 6, 6>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 6>>> hessians(input_->points.size());
+  for (std::size_t i = 0; i < input_->points.size(); i++) {
+		scores[i] = 0;
+		score_gradients[i].setZero();
+		hessians[i].setZero();
+	}
+
+	// Precompute Angular Derivatives (eq. 6.19 and 6.21)[Magnusson 2009]
+	computeAngleDerivatives(p);
+
+  std::vector<std::vector<TargetGridLeafConstPtr>> neighborhoods(num_threads_);
+  std::vector<std::vector<float>> distancess(num_threads_);
+
+	// Update gradient and hessian for each point, line 17 in Algorithm 2 [Magnusson 2009]
+#pragma omp parallel for num_threads(num_threads_) schedule(guided, 8)
+	for (std::size_t idx = 0; idx < input_->points.size(); idx++)
+	{
+		int thread_n = omp_get_thread_num();
+
+		// Original Point and Transformed Point
+		PointSource x_pt, x_trans_pt;
+		// Original Point and Transformed Point (for math)
+		Eigen::Vector3d x, x_trans;
+		// Occupied Voxel
+		TargetGridLeafConstPtr cell;
+		// Inverse Covariance of Occupied Voxel
+		Eigen::Matrix3d c_inv;
+
+		// Initialize Point Gradient and Hessian
+		Eigen::Matrix<float, 4, 6> point_gradient_;
+		Eigen::Matrix<float, 24, 6> point_hessian_;
+		point_gradient_.setZero();
+		point_gradient_.block<3, 3>(0, 0).setIdentity();
+		point_hessian_.setZero();
+
+		x_trans_pt = trans_cloud.points[idx];
+
+		auto& neighborhood = neighborhoods[thread_n];
+		auto& distances = distancess[thread_n];
+
+		// Find neighbors (Radius search has been experimentally faster than direct neighbor checking.
+		switch (search_method) {
+		case KDTREE:
+			target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
+			break;
+		case DIRECT26:
+			target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood);
+			break;
+		default:
+		case DIRECT7:
+			target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood);
+			break;
+		case DIRECT1:
+			target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood);
+			break;
+		}
+
+		double score_pt = 0;
+		Eigen::Matrix<double, 6, 1> score_gradient_pt = Eigen::Matrix<double, 6, 1>::Zero();
+		Eigen::Matrix<double, 6, 6> hessian_pt = Eigen::Matrix<double, 6, 6>::Zero();
+
+		for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin(); neighborhood_it != neighborhood.end(); neighborhood_it++)
+		{
+			cell = *neighborhood_it;
+			x_pt = input_->points[idx];
+			x = Eigen::Vector3d(x_pt.x, x_pt.y, x_pt.z);
+
+			x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
+
+			// Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
+			x_trans -= cell->getMean();
+			// Uses precomputed covariance for speed.
+			c_inv = cell->getInverseCov();
+
+			// Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009]
+			computePointDerivatives(x, point_gradient_, point_hessian_);
+			// Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009]
+			score_pt += updateDerivatives(score_gradient_pt, hessian_pt, point_gradient_, point_hessian_, x_trans, c_inv, compute_hessian);
+		}
+
+		scores[idx] = score_pt;
+		score_gradients[idx].noalias() = score_gradient_pt;
+		hessians[idx].noalias() = hessian_pt;
+	}
+
+  // Ensure that the result is invariant against the summing up order
+  for (std::size_t i = 0; i < input_->points.size(); i++) {
+		score += scores[i];
+		score_gradient += score_gradients[i];
+		hessian += hessians[i];
+	}
+
+	return (score);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeAngleDerivatives(Eigen::Matrix<double, 6, 1> &p, bool compute_hessian)
+{
+	// Simplified math for near 0 angles
+	double cx, cy, cz, sx, sy, sz;
+	if (fabs(p(3)) < 10e-5)
+	{
+		//p(3) = 0;
+		cx = 1.0;
+		sx = 0.0;
+	}
+	else
+	{
+		cx = cos(p(3));
+		sx = sin(p(3));
+	}
+	if (fabs(p(4)) < 10e-5)
+	{
+		//p(4) = 0;
+		cy = 1.0;
+		sy = 0.0;
+	}
+	else
+	{
+		cy = cos(p(4));
+		sy = sin(p(4));
+	}
+
+	if (fabs(p(5)) < 10e-5)
+	{
+		//p(5) = 0;
+		cz = 1.0;
+		sz = 0.0;
+	}
+	else
+	{
+		cz = cos(p(5));
+		sz = sin(p(5));
+	}
+
+	// Precomputed angular gradiant components. Letters correspond to Equation 6.19 [Magnusson 2009]
+	j_ang_a_ << (-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy);
+	j_ang_b_ << (cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy);
+	j_ang_c_ << (-sy * cz), sy * sz, cy;
+	j_ang_d_ << sx * cy * cz, (-sx * cy * sz), sx * sy;
+	j_ang_e_ << (-cx * cy * cz), cx * cy * sz, (-cx * sy);
+	j_ang_f_ << (-cy * sz), (-cy * cz), 0;
+	j_ang_g_ << (cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0;
+	j_ang_h_ << (sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0;
+
+	j_ang.setZero();
+	j_ang.row(0).noalias() = Eigen::Vector4f((-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy), 0.0f);
+	j_ang.row(1).noalias() = Eigen::Vector4f((cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy), 0.0f);
+	j_ang.row(2).noalias() = Eigen::Vector4f((-sy * cz), sy * sz, cy, 0.0f);
+	j_ang.row(3).noalias() = Eigen::Vector4f(sx * cy * cz, (-sx * cy * sz), sx * sy, 0.0f);
+	j_ang.row(4).noalias() = Eigen::Vector4f((-cx * cy * cz), cx * cy * sz, (-cx * sy), 0.0f);
+	j_ang.row(5).noalias() = Eigen::Vector4f((-cy * sz), (-cy * cz), 0, 0.0f);
+	j_ang.row(6).noalias() = Eigen::Vector4f((cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0, 0.0f);
+	j_ang.row(7).noalias() = Eigen::Vector4f((sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0, 0.0f);
+
+	if (compute_hessian)
+	{
+		// Precomputed angular hessian components. Letters correspond to Equation 6.21 and numbers correspond to row index [Magnusson 2009]
+		h_ang_a2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy;
+		h_ang_a3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy);
+
+		h_ang_b2_ << (cx * cy * cz), (-cx * cy * sz), (cx * sy);
+		h_ang_b3_ << (sx * cy * cz), (-sx * cy * sz), (sx * sy);
+
+		// The sign of 'sx * sz' in c2 is incorrect in [Magnusson 2009], and it is fixed here.
+		h_ang_c2_ << (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0;
+		h_ang_c3_ << (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0;
+
+		h_ang_d1_ << (-cy * cz), (cy * sz), (-sy);
+		h_ang_d2_ << (-sx * sy * cz), (sx * sy * sz), (sx * cy);
+		h_ang_d3_ << (cx * sy * cz), (-cx * sy * sz), (-cx * cy);
+
+		h_ang_e1_ << (sy * sz), (sy * cz), 0;
+		h_ang_e2_ << (-sx * cy * sz), (-sx * cy * cz), 0;
+		h_ang_e3_ << (cx * cy * sz), (cx * cy * cz), 0;
+
+		h_ang_f1_ << (-cy * cz), (cy * sz), 0;
+		h_ang_f2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0;
+		h_ang_f3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0;
+
+		h_ang.setZero();
+		h_ang.row(0).noalias() = Eigen::Vector4f((-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy, 0.0f);		// a2
+		h_ang.row(1).noalias() = Eigen::Vector4f((-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy), 0.0f);	// a3
+
+		h_ang.row(2).noalias() = Eigen::Vector4f((cx * cy * cz), (-cx * cy * sz), (cx * sy), 0.0f);							// b2
+		h_ang.row(3).noalias() = Eigen::Vector4f((sx * cy * cz), (-sx * cy * sz), (sx * sy), 0.0f);							// b3
+
+		h_ang.row(4).noalias() = Eigen::Vector4f((-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0, 0.0f);				// c2
+		h_ang.row(5).noalias() = Eigen::Vector4f((cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0, 0.0f);				// c3
+
+		h_ang.row(6).noalias() = Eigen::Vector4f((-cy * cz), (cy * sz), (sy), 0.0f);										// d1
+		h_ang.row(7).noalias() = Eigen::Vector4f((-sx * sy * cz), (sx * sy * sz), (sx * cy), 0.0f);							// d2
+		h_ang.row(8).noalias() = Eigen::Vector4f((cx * sy * cz), (-cx * sy * sz), (-cx * cy), 0.0f);						// d3
+
+		h_ang.row(9).noalias() = Eigen::Vector4f((sy * sz), (sy * cz), 0, 0.0f);											// e1
+		h_ang.row(10).noalias() = Eigen::Vector4f ((-sx * cy * sz), (-sx * cy * cz), 0, 0.0f);								// e2
+		h_ang.row(11).noalias() = Eigen::Vector4f ((cx * cy * sz), (cx * cy * cz), 0, 0.0f);								// e3
+
+		h_ang.row(12).noalias() = Eigen::Vector4f ((-cy * cz), (cy * sz), 0, 0.0f);											// f1
+		h_ang.row(13).noalias() = Eigen::Vector4f ((-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0, 0.0f);			// f2
+		h_ang.row(14).noalias() = Eigen::Vector4f ((-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0, 0.0f);			// f3
+	}
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<float, 4, 6>& point_gradient_, Eigen::Matrix<float, 24, 6>& point_hessian_, bool compute_hessian) const
+{
+	Eigen::Vector4f x4(x[0], x[1], x[2], 0.0f);
+
+	// Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p.
+	// Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson 2009]
+	Eigen::Matrix<float, 8, 1> x_j_ang = j_ang * x4;
+
+	point_gradient_(1, 3) = x_j_ang[0];
+	point_gradient_(2, 3) = x_j_ang[1];
+	point_gradient_(0, 4) = x_j_ang[2];
+	point_gradient_(1, 4) = x_j_ang[3];
+	point_gradient_(2, 4) = x_j_ang[4];
+	point_gradient_(0, 5) = x_j_ang[5];
+	point_gradient_(1, 5) = x_j_ang[6];
+	point_gradient_(2, 5) = x_j_ang[7];
+
+	if (compute_hessian)
+	{
+		Eigen::Matrix<float, 16, 1> x_h_ang = h_ang * x4;
+
+		// Vectors from Equation 6.21 [Magnusson 2009]
+		Eigen::Vector4f a (0, x_h_ang[0], x_h_ang[1], 0.0f);
+		Eigen::Vector4f b (0, x_h_ang[2], x_h_ang[3], 0.0f);
+		Eigen::Vector4f c (0, x_h_ang[4], x_h_ang[5], 0.0f);
+		Eigen::Vector4f d (x_h_ang[6], x_h_ang[7], x_h_ang[8], 0.0f);
+		Eigen::Vector4f e (x_h_ang[9], x_h_ang[10], x_h_ang[11], 0.0f);
+		Eigen::Vector4f f (x_h_ang[12], x_h_ang[13], x_h_ang[14], 0.0f);
+
+		// Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p.
+		// Derivative w.r.t. ith and jth elements of transform vector corresponds to the 3x1 block matrix starting at (3i,j), Equation 6.20 and 6.21 [Magnusson 2009]
+		point_hessian_.block<4, 1>((9/3)*4, 3) = a;
+		point_hessian_.block<4, 1>((12/3)*4, 3) = b;
+		point_hessian_.block<4, 1>((15/3)*4, 3) = c;
+		point_hessian_.block<4, 1>((9/3)*4, 4) = b;
+		point_hessian_.block<4, 1>((12/3)*4, 4) = d;
+		point_hessian_.block<4, 1>((15/3)*4, 4) = e;
+		point_hessian_.block<4, 1>((9/3)*4, 5) = c;
+		point_hessian_.block<4, 1>((12/3)*4, 5) = e;
+		point_hessian_.block<4, 1>((15/3)*4, 5) = f;
+	}
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<double, 3, 6>& point_gradient_, Eigen::Matrix<double, 18, 6>& point_hessian_, bool compute_hessian) const
+{
+	// Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p.
+	// Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson 2009]
+	point_gradient_(1, 3) = x.dot(j_ang_a_);
+	point_gradient_(2, 3) = x.dot(j_ang_b_);
+	point_gradient_(0, 4) = x.dot(j_ang_c_);
+	point_gradient_(1, 4) = x.dot(j_ang_d_);
+	point_gradient_(2, 4) = x.dot(j_ang_e_);
+	point_gradient_(0, 5) = x.dot(j_ang_f_);
+	point_gradient_(1, 5) = x.dot(j_ang_g_);
+	point_gradient_(2, 5) = x.dot(j_ang_h_);
+
+	if (compute_hessian)
+	{
+		// Vectors from Equation 6.21 [Magnusson 2009]
+		Eigen::Vector3d a, b, c, d, e, f;
+
+		a << 0, x.dot(h_ang_a2_), x.dot(h_ang_a3_);
+		b << 0, x.dot(h_ang_b2_), x.dot(h_ang_b3_);
+		c << 0, x.dot(h_ang_c2_), x.dot(h_ang_c3_);
+		d << x.dot(h_ang_d1_), x.dot(h_ang_d2_), x.dot(h_ang_d3_);
+		e << x.dot(h_ang_e1_), x.dot(h_ang_e2_), x.dot(h_ang_e3_);
+		f << x.dot(h_ang_f1_), x.dot(h_ang_f2_), x.dot(h_ang_f3_);
+
+		// Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p.
+		// Derivative w.r.t. ith and jth elements of transform vector corresponds to the 3x1 block matrix starting at (3i,j), Equation 6.20 and 6.21 [Magnusson 2009]
+		point_hessian_.block<3, 1>(9, 3) = a;
+		point_hessian_.block<3, 1>(12, 3) = b;
+		point_hessian_.block<3, 1>(15, 3) = c;
+		point_hessian_.block<3, 1>(9, 4) = b;
+		point_hessian_.block<3, 1>(12, 4) = d;
+		point_hessian_.block<3, 1>(15, 4) = e;
+		point_hessian_.block<3, 1>(9, 5) = c;
+		point_hessian_.block<3, 1>(12, 5) = e;
+		point_hessian_.block<3, 1>(15, 5) = f;
+	}
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> double
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::updateDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient,
+	Eigen::Matrix<double, 6, 6> &hessian,
+	const Eigen::Matrix<float, 4, 6> &point_gradient4,
+	const Eigen::Matrix<float, 24, 6> &point_hessian_,
+	const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv,
+	bool compute_hessian) const
+{
+	Eigen::Matrix<float, 1, 4> x_trans4( x_trans[0], x_trans[1], x_trans[2], 0.0f );
+	Eigen::Matrix4f c_inv4 = Eigen::Matrix4f::Zero();
+	c_inv4.topLeftCorner(3, 3) = c_inv.cast<float>();
+
+	float gauss_d2 = gauss_d2_;
+
+	// e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
+	float e_x_cov_x = exp(-gauss_d2 * x_trans4.dot(x_trans4 * c_inv4) * 0.5f);
+	// Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009]
+	float score_inc = -gauss_d1_ * e_x_cov_x;
+
+	e_x_cov_x = gauss_d2 * e_x_cov_x;
+
+	// Error checking for invalid values.
+	if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x)
+		return (0);
+
+	// Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
+	e_x_cov_x *= gauss_d1_;
+
+	Eigen::Matrix<float, 4, 6> c_inv4_x_point_gradient4 = c_inv4 * point_gradient4;
+	Eigen::Matrix<float, 6, 1> x_trans4_dot_c_inv4_x_point_gradient4 = x_trans4 * c_inv4_x_point_gradient4;
+
+	score_gradient.noalias() += (e_x_cov_x * x_trans4_dot_c_inv4_x_point_gradient4).cast<double>();
+
+	if (compute_hessian) {
+		Eigen::Matrix<float, 1, 4> x_trans4_x_c_inv4 = x_trans4 * c_inv4;
+		Eigen::Matrix<float, 6, 6> point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i = point_gradient4.transpose() * c_inv4_x_point_gradient4;
+		Eigen::Matrix<float, 6, 1> x_trans4_dot_c_inv4_x_ext_point_hessian_4ij;
+
+		for (int i = 0; i < 6; i++) {
+			// Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
+			// Update gradient, Equation 6.12 [Magnusson 2009]
+			x_trans4_dot_c_inv4_x_ext_point_hessian_4ij.noalias() = x_trans4_x_c_inv4 * point_hessian_.block<4, 6>(i * 4, 0);
+
+			for (int j = 0; j < hessian.cols(); j++) {
+				// Update hessian, Equation 6.13 [Magnusson 2009]
+				hessian(i, j) += e_x_cov_x * (-gauss_d2 * x_trans4_dot_c_inv4_x_point_gradient4(i) * x_trans4_dot_c_inv4_x_point_gradient4(j) +
+					x_trans4_dot_c_inv4_x_ext_point_hessian_4ij(j) +
+					point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i(j, i));
+			}
+		}
+	}
+
+	return (score_inc);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeHessian (Eigen::Matrix<double, 6, 6> &hessian,
+                                                                             PointCloudSource &trans_cloud, Eigen::Matrix<double, 6, 1> &)
+{
+  // Original Point and Transformed Point
+  PointSource x_pt, x_trans_pt;
+  // Original Point and Transformed Point (for math)
+  Eigen::Vector3d x, x_trans;
+  // Occupied Voxel
+  TargetGridLeafConstPtr cell;
+  // Inverse Covariance of Occupied Voxel
+  Eigen::Matrix3d c_inv;
+
+  // Initialize Point Gradient and Hessian
+  Eigen::Matrix<double, 3, 6> point_gradient_;
+  Eigen::Matrix<double, 18, 6> point_hessian_;
+  point_gradient_.setZero();
+  point_gradient_.block<3, 3>(0, 0).setIdentity();
+  point_hessian_.setZero();
+
+  hessian.setZero ();
+
+  // Precompute Angular Derivatives unnecessary because only used after regular derivative calculation
+
+  // Update hessian for each point, line 17 in Algorithm 2 [Magnusson 2009]
+  for (size_t idx = 0; idx < input_->points.size (); idx++)
+  {
+    x_trans_pt = trans_cloud.points[idx];
+
+    // Find neighbors (Radius search has been experimentally faster than direct neighbor checking.
+    std::vector<TargetGridLeafConstPtr> neighborhood;
+    std::vector<float> distances;
+		switch (search_method) {
+		case KDTREE:
+			target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
+			break;
+		case DIRECT26:
+			target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood);
+			break;
+		default:
+		case DIRECT7:
+			target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood);
+			break;
+		case DIRECT1:
+			target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood);
+			break;
+		}
+
+    for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++)
+    {
+      cell = *neighborhood_it;
+
+      {
+        x_pt = input_->points[idx];
+        x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z);
+
+        x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
+
+        // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
+        x_trans -= cell->getMean ();
+        // Uses precomputed covariance for speed.
+        c_inv = cell->getInverseCov ();
+
+        // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009]
+        computePointDerivatives (x, point_gradient_, point_hessian_);
+        // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009]
+        updateHessian (hessian, point_gradient_, point_hessian_, x_trans, c_inv);
+      }
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::updateHessian (Eigen::Matrix<double, 6, 6> &hessian,
+	const Eigen::Matrix<double, 3, 6> &point_gradient_,
+	const Eigen::Matrix<double, 18, 6> &point_hessian_,
+	const Eigen::Vector3d &x_trans,
+	const Eigen::Matrix3d &c_inv) const
+{
+  Eigen::Vector3d cov_dxd_pi;
+  // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
+  double e_x_cov_x = gauss_d2_ * exp (-gauss_d2_ * x_trans.dot (c_inv * x_trans) / 2);
+
+  // Error checking for invalid values.
+  if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x)
+    return;
+
+  // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
+  e_x_cov_x *= gauss_d1_;
+
+  for (int i = 0; i < 6; i++)
+  {
+    // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
+    cov_dxd_pi = c_inv * point_gradient_.col (i);
+
+    for (int j = 0; j < hessian.cols (); j++)
+    {
+      // Update hessian, Equation 6.13 [Magnusson 2009]
+      hessian (i, j) += e_x_cov_x * (-gauss_d2_ * x_trans.dot (cov_dxd_pi) * x_trans.dot (c_inv * point_gradient_.col (j)) +
+                                  x_trans.dot (c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
+                                  point_gradient_.col (j).dot (cov_dxd_pi) );
+    }
+  }
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> bool
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::updateIntervalMT (double &a_l, double &f_l, double &g_l,
+                                                                               double &a_u, double &f_u, double &g_u,
+                                                                               double a_t, double f_t, double g_t)
+{
+  // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente 1994]
+  if (f_t > f_l)
+  {
+    a_u = a_t;
+    f_u = f_t;
+    g_u = g_t;
+    return (false);
+  }
+  // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994]
+  else
+  if (g_t * (a_l - a_t) > 0)
+  {
+    a_l = a_t;
+    f_l = f_t;
+    g_l = g_t;
+    return (false);
+  }
+  // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994]
+  else
+  if (g_t * (a_l - a_t) < 0)
+  {
+    a_u = a_l;
+    f_u = f_l;
+    g_u = g_l;
+
+    a_l = a_t;
+    f_l = f_t;
+    g_l = g_t;
+    return (false);
+  }
+  // Interval Converged
+  else
+    return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> double
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::trialValueSelectionMT (double a_l, double f_l, double g_l,
+                                                                                    double a_u, double f_u, double g_u,
+                                                                                    double a_t, double f_t, double g_t)
+{
+  // Case 1 in Trial Value Selection [More, Thuente 1994]
+  if (f_t > f_l)
+  {
+    // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
+    // Equation 2.4.52 [Sun, Yuan 2006]
+    double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
+    double w = std::sqrt (z * z - g_t * g_l);
+    // Equation 2.4.56 [Sun, Yuan 2006]
+    double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
+
+    // Calculate the minimizer of the quadratic that interpolates f_l, f_t and g_l
+    // Equation 2.4.2 [Sun, Yuan 2006]
+    double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t));
+
+    if (std::fabs (a_c - a_l) < std::fabs (a_q - a_l))
+      return (a_c);
+    else
+      return (0.5 * (a_q + a_c));
+  }
+  // Case 2 in Trial Value Selection [More, Thuente 1994]
+  else
+  if (g_t * g_l < 0)
+  {
+    // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
+    // Equation 2.4.52 [Sun, Yuan 2006]
+    double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
+    double w = std::sqrt (z * z - g_t * g_l);
+    // Equation 2.4.56 [Sun, Yuan 2006]
+    double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
+
+    // Calculate the minimizer of the quadratic that interpolates f_l, g_l and g_t
+    // Equation 2.4.5 [Sun, Yuan 2006]
+    double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
+
+    if (std::fabs (a_c - a_t) >= std::fabs (a_s - a_t))
+      return (a_c);
+    else
+      return (a_s);
+  }
+  // Case 3 in Trial Value Selection [More, Thuente 1994]
+  else
+  if (std::fabs (g_t) <= std::fabs (g_l))
+  {
+    // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
+    // Equation 2.4.52 [Sun, Yuan 2006]
+    double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
+    double w = std::sqrt (z * z - g_t * g_l);
+    double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
+
+    // Calculate the minimizer of the quadratic that interpolates g_l and g_t
+    // Equation 2.4.5 [Sun, Yuan 2006]
+    double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
+
+    double a_t_next;
+
+    if (std::fabs (a_c - a_t) < std::fabs (a_s - a_t))
+      a_t_next = a_c;
+    else
+      a_t_next = a_s;
+
+    if (a_t > a_l)
+      return (std::min (a_t + 0.66 * (a_u - a_t), a_t_next));
+    else
+      return (std::max (a_t + 0.66 * (a_u - a_t), a_t_next));
+  }
+  // Case 4 in Trial Value Selection [More, Thuente 1994]
+  else
+  {
+    // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t
+    // Equation 2.4.52 [Sun, Yuan 2006]
+    double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u;
+    double w = std::sqrt (z * z - g_t * g_u);
+    // Equation 2.4.56 [Sun, Yuan 2006]
+    return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w));
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> double
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT (const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix<double, 6, 1> &step_dir, double step_init, double step_max,
+                                                                                  double step_min, double &score, Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
+                                                                                  PointCloudSource &trans_cloud)
+{
+  // Set the value of phi(0), Equation 1.3 [More, Thuente 1994]
+  double phi_0 = -score;
+  // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994]
+  double d_phi_0 = -(score_gradient.dot (step_dir));
+
+  Eigen::Matrix<double, 6, 1>  x_t;
+
+  if (d_phi_0 >= 0)
+  {
+    // Not a decent direction
+    if (d_phi_0 == 0)
+      return 0;
+    else
+    {
+      // Reverse step direction and calculate optimal step.
+      d_phi_0 *= -1;
+      step_dir *= -1;
+
+    }
+  }
+
+  // The Search Algorithm for T(mu) [More, Thuente 1994]
+
+  int max_step_iterations = 10;
+  int step_iterations = 0;
+
+  // Sufficient decrease constant, Equation 1.1 [More, Thuete 1994]
+  double mu = 1.e-4;
+  // Curvature condition constant, Equation 1.2 [More, Thuete 1994]
+  double nu = 0.9;
+
+  // Initial endpoints of Interval I,
+  double a_l = 0, a_u = 0;
+
+  // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 [More, Thuente 1994]
+  double f_l = auxiliaryFunction_PsiMT (a_l, phi_0, phi_0, d_phi_0, mu);
+  double g_l = auxiliaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
+
+  double f_u = auxiliaryFunction_PsiMT (a_u, phi_0, phi_0, d_phi_0, mu);
+  double g_u = auxiliaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
+
+  // Check used to allow More-Thuente step length calculation to be skipped by making step_min == step_max
+  bool interval_converged = (step_max - step_min) < 0, open_interval = true;
+
+  double a_t = step_init;
+  a_t = std::min (a_t, step_max);
+  a_t = std::max (a_t, step_min);
+
+  x_t = x + step_dir * a_t;
+
+  final_transformation_ = (Eigen::Translation<float, 3>(static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
+                           Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
+                           Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
+                           Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
+
+  // New transformed point cloud
+  transformPointCloud (*input_, trans_cloud, final_transformation_);
+
+  // Updates score, gradient and hessian.  Hessian calculation is unnecessary but testing showed that most step calculations use the
+  // initial step suggestion and recalculation the reusable portions of the hessian would intail more computation time.
+  score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, true);
+
+  // Calculate phi(alpha_t)
+  double phi_t = -score;
+  // Calculate phi'(alpha_t)
+  double d_phi_t = -(score_gradient.dot (step_dir));
+
+  // Calculate psi(alpha_t)
+  double psi_t = auxiliaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
+  // Calculate psi'(alpha_t)
+  double d_psi_t = auxiliaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);
+
+  // Iterate until max number of iterations, interval convergence or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994]
+  while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/))
+  {
+    // Use auxiliary function if interval I is not closed
+    if (open_interval)
+    {
+      a_t = trialValueSelectionMT (a_l, f_l, g_l,
+                                   a_u, f_u, g_u,
+                                   a_t, psi_t, d_psi_t);
+    }
+    else
+    {
+      a_t = trialValueSelectionMT (a_l, f_l, g_l,
+                                   a_u, f_u, g_u,
+                                   a_t, phi_t, d_phi_t);
+    }
+
+    a_t = std::min (a_t, step_max);
+    a_t = std::max (a_t, step_min);
+
+    x_t = x + step_dir * a_t;
+
+    final_transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
+                             Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
+                             Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
+                             Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
+
+    // New transformed point cloud
+    // Done on final cloud to prevent wasted computation
+    transformPointCloud (*input_, trans_cloud, final_transformation_);
+
+    // Updates score, gradient. Values stored to prevent wasted computation.
+    score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, false);
+
+    // Calculate phi(alpha_t+)
+    phi_t = -score;
+    // Calculate phi'(alpha_t+)
+    d_phi_t = -(score_gradient.dot (step_dir));
+
+    // Calculate psi(alpha_t+)
+    psi_t = auxiliaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
+    // Calculate psi'(alpha_t+)
+    d_psi_t = auxiliaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);
+
+    // Check if I is now a closed interval
+    if (open_interval && (psi_t <= 0 && d_psi_t >= 0))
+    {
+      open_interval = false;
+
+      // Converts f_l and g_l from psi to phi
+      f_l = f_l + phi_0 - mu * d_phi_0 * a_l;
+      g_l = g_l + mu * d_phi_0;
+
+      // Converts f_u and g_u from psi to phi
+      f_u = f_u + phi_0 - mu * d_phi_0 * a_u;
+      g_u = g_u + mu * d_phi_0;
+    }
+
+    if (open_interval)
+    {
+      // Update interval end points using Updating Algorithm [More, Thuente 1994]
+      interval_converged = updateIntervalMT (a_l, f_l, g_l,
+                                             a_u, f_u, g_u,
+                                             a_t, psi_t, d_psi_t);
+    }
+    else
+    {
+      // Update interval end points using Modified Updating Algorithm [More, Thuente 1994]
+      interval_converged = updateIntervalMT (a_l, f_l, g_l,
+                                             a_u, f_u, g_u,
+                                             a_t, phi_t, d_phi_t);
+    }
+
+    step_iterations++;
+  }
+
+  // If inner loop was run then hessian needs to be calculated.
+  // Hessian is unnecessary for step length determination but gradients are required
+  // so derivative and transform data is stored for the next iteration.
+  if (step_iterations)
+    computeHessian (hessian, trans_cloud, x_t);
+
+  return (a_t);
+}
+
+
+template<typename PointSource, typename PointTarget>
+double pclomp::NormalDistributionsTransform<PointSource, PointTarget>::calculateScore(const PointCloudSource & trans_cloud) const
+{
+	double score = 0;
+
+	for (std::size_t idx = 0; idx < trans_cloud.points.size(); idx++)
+	{
+		PointSource x_trans_pt = trans_cloud.points[idx];
+
+		// Find neighbors (Radius search has been experimentally faster than direct neighbor checking.
+		std::vector<TargetGridLeafConstPtr> neighborhood;
+		std::vector<float> distances;
+		switch (search_method) {
+		case KDTREE:
+			target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
+			break;
+		case DIRECT26:
+			target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood);
+			break;
+		default:
+		case DIRECT7:
+			target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood);
+			break;
+		case DIRECT1:
+			target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood);
+			break;
+		}
+
+		for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin(); neighborhood_it != neighborhood.end(); neighborhood_it++)
+		{
+			TargetGridLeafConstPtr cell = *neighborhood_it;
+
+			Eigen::Vector3d x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
+
+			// Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
+			x_trans -= cell->getMean();
+			// Uses precomputed covariance for speed.
+			Eigen::Matrix3d c_inv = cell->getInverseCov();
+
+			// e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
+			double e_x_cov_x = exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2);
+			// Calculate probability of transformed points existence, Equation 6.9 [Magnusson 2009]
+			double score_inc = -gauss_d1_ * e_x_cov_x - gauss_d3_;
+
+			score += score_inc / neighborhood.size();
+		}
+	}
+	return (score) / static_cast<double> (trans_cloud.size());
+}
+
+#endif // PCL_REGISTRATION_NDT_IMPL_H_

src/detection/detection_ndt_pclomp/pclomp/pclomp/ndt_omp.h

@@ -0,0 +1,506 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-2012, Willow Garage, Inc.
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_REGISTRATION_NDT_OMP_H_
+#define PCL_REGISTRATION_NDT_OMP_H_
+
+#include <pcl/registration/registration.h>
+#include <pcl/search/impl/search.hpp>
+#include <pclomp/voxel_grid_covariance_omp.h>
+
+#include <unsupported/Eigen/NonLinearOptimization>
+
+namespace pclomp
+{
+	enum NeighborSearchMethod {
+		KDTREE,
+		DIRECT26,
+		DIRECT7,
+		DIRECT1
+	};
+
+	/** \brief A 3D Normal Distribution Transform registration implementation for point cloud data.
+	  * \note For more information please see
+	  * <b>Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform —
+	  * an Efficient Representation for Registration, Surface Analysis, and Loop Detection.
+	  * PhD thesis, Orebro University. Orebro Studies in Technology 36.</b>,
+	  * <b>More, J., and Thuente, D. (1994). Line Search Algorithm with Guaranteed Sufficient Decrease
+	  * In ACM Transactions on Mathematical Software.</b> and
+	  * Sun, W. and Yuan, Y, (2006) Optimization Theory and Methods: Nonlinear Programming. 89-100
+	  * \note Math refactored by Todor Stoyanov.
+	  * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
+	  */
+	template<typename PointSource, typename PointTarget>
+	class NormalDistributionsTransform : public pcl::Registration<PointSource, PointTarget>
+	{
+	protected:
+
+		typedef typename pcl::Registration<PointSource, PointTarget>::PointCloudSource PointCloudSource;
+		typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
+		typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
+
+		typedef typename pcl::Registration<PointSource, PointTarget>::PointCloudTarget PointCloudTarget;
+		typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
+		typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;
+
+		typedef pcl::PointIndices::Ptr PointIndicesPtr;
+		typedef pcl::PointIndices::ConstPtr PointIndicesConstPtr;
+
+		/** \brief Typename of searchable voxel grid containing mean and covariance. */
+		typedef pclomp::VoxelGridCovariance<PointTarget> TargetGrid;
+		/** \brief Typename of pointer to searchable voxel grid. */
+		typedef TargetGrid* TargetGridPtr;
+		/** \brief Typename of const pointer to searchable voxel grid. */
+		typedef const TargetGrid* TargetGridConstPtr;
+		/** \brief Typename of const pointer to searchable voxel grid leaf. */
+		typedef typename TargetGrid::LeafConstPtr TargetGridLeafConstPtr;
+
+
+	public:
+
+#if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
+		typedef pcl::shared_ptr< NormalDistributionsTransform<PointSource, PointTarget> > Ptr;
+		typedef pcl::shared_ptr< const NormalDistributionsTransform<PointSource, PointTarget> > ConstPtr;
+#else
+		typedef boost::shared_ptr< NormalDistributionsTransform<PointSource, PointTarget> > Ptr;
+		typedef boost::shared_ptr< const NormalDistributionsTransform<PointSource, PointTarget> > ConstPtr;
+#endif
+
+
+		/** \brief Constructor.
+		  * Sets \ref outlier_ratio_ to 0.35, \ref step_size_ to 0.05 and \ref resolution_ to 1.0
+		  */
+		NormalDistributionsTransform();
+
+		/** \brief Empty destructor */
+		virtual ~NormalDistributionsTransform() {}
+
+    void setNumThreads(int n) {
+      num_threads_ = n;
+    }
+
+		/** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to).
+		  * \param[in] cloud the input point cloud target
+		  */
+		inline void
+			setInputTarget(const PointCloudTargetConstPtr &cloud)
+		{
+			pcl::Registration<PointSource, PointTarget>::setInputTarget(cloud);
+			init();
+		}
+
+		/** \brief Set/change the voxel grid resolution.
+		  * \param[in] resolution side length of voxels
+		  */
+		inline void
+			setResolution(float resolution)
+		{
+			// Prevents unnecessary voxel initiations
+			if (resolution_ != resolution)
+			{
+				resolution_ = resolution;
+				if (input_)
+					init();
+			}
+		}
+
+		/** \brief Get voxel grid resolution.
+		  * \return side length of voxels
+		  */
+		inline float
+			getResolution() const
+		{
+			return (resolution_);
+		}
+
+		/** \brief Get the newton line search maximum step length.
+		  * \return maximum step length
+		  */
+		inline double
+			getStepSize() const
+		{
+			return (step_size_);
+		}
+
+		/** \brief Set/change the newton line search maximum step length.
+		  * \param[in] step_size maximum step length
+		  */
+		inline void
+			setStepSize(double step_size)
+		{
+			step_size_ = step_size;
+		}
+
+		/** \brief Get the point cloud outlier ratio.
+		  * \return outlier ratio
+		  */
+		inline double
+			getOutlierRatio() const
+		{
+			return (outlier_ratio_);
+		}
+
+		/** \brief Set/change the point cloud outlier ratio.
+		  * \param[in] outlier_ratio outlier ratio
+		  */
+		inline void
+			setOutlierRatio(double outlier_ratio)
+		{
+			outlier_ratio_ = outlier_ratio;
+		}
+
+		inline void setNeighborhoodSearchMethod(NeighborSearchMethod method) {
+			search_method = method;
+		}
+
+		/** \brief Get the registration alignment probability.
+		  * \return transformation probability
+		  */
+		inline double
+			getTransformationProbability() const
+		{
+			return (trans_probability_);
+		}
+
+		/** \brief Get the number of iterations required to calculate alignment.
+		  * \return final number of iterations
+		  */
+		inline int
+			getFinalNumIteration() const
+		{
+			return (nr_iterations_);
+		}
+
+		/** \brief Convert 6 element transformation vector to affine transformation.
+		  * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
+		  * \param[out] trans affine transform corresponding to given transformation vector
+		  */
+		static void
+			convertTransform(const Eigen::Matrix<double, 6, 1> &x, Eigen::Affine3f &trans)
+		{
+			trans = Eigen::Translation<float, 3>(float(x(0)), float(x(1)), float(x(2))) *
+				Eigen::AngleAxis<float>(float(x(3)), Eigen::Vector3f::UnitX()) *
+				Eigen::AngleAxis<float>(float(x(4)), Eigen::Vector3f::UnitY()) *
+				Eigen::AngleAxis<float>(float(x(5)), Eigen::Vector3f::UnitZ());
+		}
+
+		/** \brief Convert 6 element transformation vector to transformation matrix.
+		  * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
+		  * \param[out] trans 4x4 transformation matrix corresponding to given transformation vector
+		  */
+		static void
+			convertTransform(const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix4f &trans)
+		{
+			Eigen::Affine3f _affine;
+			convertTransform(x, _affine);
+			trans = _affine.matrix();
+		}
+
+		// negative log likelihood function
+		// lower is better
+		double calculateScore(const PointCloudSource& cloud) const;
+
+	protected:
+
+		using pcl::Registration<PointSource, PointTarget>::reg_name_;
+		using pcl::Registration<PointSource, PointTarget>::getClassName;
+		using pcl::Registration<PointSource, PointTarget>::input_;
+		using pcl::Registration<PointSource, PointTarget>::indices_;
+		using pcl::Registration<PointSource, PointTarget>::target_;
+		using pcl::Registration<PointSource, PointTarget>::nr_iterations_;
+		using pcl::Registration<PointSource, PointTarget>::max_iterations_;
+		using pcl::Registration<PointSource, PointTarget>::previous_transformation_;
+		using pcl::Registration<PointSource, PointTarget>::final_transformation_;
+		using pcl::Registration<PointSource, PointTarget>::transformation_;
+		using pcl::Registration<PointSource, PointTarget>::transformation_epsilon_;
+		using pcl::Registration<PointSource, PointTarget>::converged_;
+		using pcl::Registration<PointSource, PointTarget>::corr_dist_threshold_;
+		using pcl::Registration<PointSource, PointTarget>::inlier_threshold_;
+
+		using pcl::Registration<PointSource, PointTarget>::update_visualizer_;
+
+		/** \brief Estimate the transformation and returns the transformed source (input) as output.
+		  * \param[out] output the resultant input transformed point cloud dataset
+		  */
+		virtual void
+			computeTransformation(PointCloudSource &output)
+		{
+			computeTransformation(output, Eigen::Matrix4f::Identity());
+		}
+
+		/** \brief Estimate the transformation and returns the transformed source (input) as output.
+		  * \param[out] output the resultant input transformed point cloud dataset
+		  * \param[in] guess the initial gross estimation of the transformation
+		  */
+		virtual void
+			computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess);
+
+		/** \brief Initiate covariance voxel structure. */
+		void inline
+			init()
+		{
+			target_cells_.setLeafSize(resolution_, resolution_, resolution_);
+			target_cells_.setInputCloud(target_);
+			// Initiate voxel structure.
+			target_cells_.filter(true);
+		}
+
+		/** \brief Compute derivatives of probability function w.r.t. the transformation vector.
+		  * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
+		  * \param[out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector
+		  * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
+		  * \param[in] trans_cloud transformed point cloud
+		  * \param[in] p the current transform vector
+		  * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation.
+		  */
+		double
+			computeDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient,
+				Eigen::Matrix<double, 6, 6> &hessian,
+				PointCloudSource &trans_cloud,
+				Eigen::Matrix<double, 6, 1> &p,
+				bool compute_hessian = true);
+
+		/** \brief Compute individual point contributions to derivatives of probability function w.r.t. the transformation vector.
+		  * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
+		  * \param[in,out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector
+		  * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
+		  * \param[in] x_trans transformed point minus mean of occupied covariance voxel
+		  * \param[in] c_inv covariance of occupied covariance voxel
+		  * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation.
+		  */
+		double
+			updateDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient,
+				Eigen::Matrix<double, 6, 6> &hessian,
+				const Eigen::Matrix<float, 4, 6> &point_gradient_,
+				const Eigen::Matrix<float, 24, 6> &point_hessian_,
+				const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv,
+				bool compute_hessian = true) const;
+
+		/** \brief Precompute angular components of derivatives.
+		  * \note Equation 6.19 and 6.21 [Magnusson 2009].
+		  * \param[in] p the current transform vector
+		  * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation.
+		  */
+		void
+			computeAngleDerivatives(Eigen::Matrix<double, 6, 1> &p, bool compute_hessian = true);
+
+		/** \brief Compute point derivatives.
+		  * \note Equation 6.18-21 [Magnusson 2009].
+		  * \param[in] x point from the input cloud
+		  * \param[in] compute_hessian flag to calculate hessian, unnecessary for step calculation.
+		  */
+		void
+			computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<double, 3, 6>& point_gradient_, Eigen::Matrix<double, 18, 6>& point_hessian_, bool compute_hessian = true) const;
+
+		void
+			computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<float, 4, 6>& point_gradient_, Eigen::Matrix<float, 24, 6>& point_hessian_, bool compute_hessian = true) const;
+
+		/** \brief Compute hessian of probability function w.r.t. the transformation vector.
+		  * \note Equation 6.13 [Magnusson 2009].
+		  * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
+		  * \param[in] trans_cloud transformed point cloud
+		  * \param[in] p the current transform vector
+		  */
+		void
+			computeHessian(Eigen::Matrix<double, 6, 6> &hessian,
+				PointCloudSource &trans_cloud,
+				Eigen::Matrix<double, 6, 1> &p);
+
+		/** \brief Compute individual point contributions to hessian of probability function w.r.t. the transformation vector.
+		  * \note Equation 6.13 [Magnusson 2009].
+		  * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
+		  * \param[in] x_trans transformed point minus mean of occupied covariance voxel
+		  * \param[in] c_inv covariance of occupied covariance voxel
+		  */
+		void
+			updateHessian(Eigen::Matrix<double, 6, 6> &hessian,
+				const Eigen::Matrix<double, 3, 6> &point_gradient_,
+				const Eigen::Matrix<double, 18, 6> &point_hessian_,
+				const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv) const;
+
+		/** \brief Compute line search step length and update transform and probability derivatives using More-Thuente method.
+		  * \note Search Algorithm [More, Thuente 1994]
+		  * \param[in] x initial transformation vector, \f$ x \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \vec{p} \f$ in Algorithm 2 [Magnusson 2009]
+		  * \param[in] step_dir descent direction, \f$ p \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \delta \vec{p} \f$ normalized in Algorithm 2 [Magnusson 2009]
+		  * \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in Moore-Thuente (1994) and the normal of \f$ \delta \vec{p} \f$ in Algorithm 2 [Magnusson 2009]
+		  * \param[in] step_max maximum step length, \f$ \alpha_max \f$ in Moore-Thuente (1994)
+		  * \param[in] step_min minimum step length, \f$ \alpha_min \f$ in Moore-Thuente (1994)
+		  * \param[out] score final score function value, \f$ f(x + \alpha p) \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ score \f$ in Algorithm 2 [Magnusson 2009]
+		  * \param[in,out] score_gradient gradient of score function w.r.t. transformation vector, \f$ f'(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ \vec{g} \f$ in Algorithm 2 [Magnusson 2009]
+		  * \param[out] hessian hessian of score function w.r.t. transformation vector, \f$ f''(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ H \f$ in Algorithm 2 [Magnusson 2009]
+		  * \param[in,out] trans_cloud transformed point cloud, \f$ X \f$ transformed by \f$ T(\vec{p},\vec{x}) \f$ in Algorithm 2 [Magnusson 2009]
+		  * \return final step length
+		  */
+		double
+			computeStepLengthMT(const Eigen::Matrix<double, 6, 1> &x,
+				Eigen::Matrix<double, 6, 1> &step_dir,
+				double step_init,
+				double step_max, double step_min,
+				double &score,
+				Eigen::Matrix<double, 6, 1> &score_gradient,
+				Eigen::Matrix<double, 6, 6> &hessian,
+				PointCloudSource &trans_cloud);
+
+		/** \brief Update interval of possible step lengths for More-Thuente method, \f$ I \f$ in More-Thuente (1994)
+		  * \note Updating Algorithm until some value satisfies \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$
+		  * and Modified Updating Algorithm from then on [More, Thuente 1994].
+		  * \param[in,out] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994)
+		  * \param[in,out] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_l) \f$ for Update Algorithm and \f$ \phi(\alpha_l) \f$ for Modified Update Algorithm
+		  * \param[in,out] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_l) \f$ for Update Algorithm and \f$ \phi'(\alpha_l) \f$ for Modified Update Algorithm
+		  * \param[in,out] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994)
+		  * \param[in,out] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_u) \f$ for Update Algorithm and \f$ \phi(\alpha_u) \f$ for Modified Update Algorithm
+		  * \param[in,out] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_u) \f$ for Update Algorithm and \f$ \phi'(\alpha_u) \f$ for Modified Update Algorithm
+		  * \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994)
+		  * \param[in] f_t value at trial value, \f$ f_t \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_t) \f$ for Update Algorithm and \f$ \phi(\alpha_t) \f$ for Modified Update Algorithm
+		  * \param[in] g_t derivative at trial value, \f$ g_t \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_t) \f$ for Update Algorithm and \f$ \phi'(\alpha_t) \f$ for Modified Update Algorithm
+		  * \return if interval converges
+		  */
+		bool
+			updateIntervalMT(double &a_l, double &f_l, double &g_l,
+				double &a_u, double &f_u, double &g_u,
+				double a_t, double f_t, double g_t);
+
+		/** \brief Select new trial value for More-Thuente method.
+		  * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is used for \f$ f_k \f$ and \f$ g_k \f$
+		  * until some value satisfies the test \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$
+		  * then \f$ \phi(\alpha_k) \f$ is used from then on.
+		  * \note Interpolation Minimizer equations from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100).
+		  * \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994)
+		  * \param[in] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994)
+		  * \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994)
+		  * \param[in] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994)
+		  * \param[in] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994)
+		  * \param[in] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994)
+		  * \param[in] a_t previous trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994)
+		  * \param[in] f_t value at previous trial value, \f$ f_t \f$ in Moore-Thuente (1994)
+		  * \param[in] g_t derivative at previous trial value, \f$ g_t \f$ in Moore-Thuente (1994)
+		  * \return new trial value
+		  */
+		double
+			trialValueSelectionMT(double a_l, double f_l, double g_l,
+				double a_u, double f_u, double g_u,
+				double a_t, double f_t, double g_t);
+
+		/** \brief Auxiliary function used to determine endpoints of More-Thuente interval.
+		  * \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994)
+		  * \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994)
+		  * \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in More-Thuente (1994)
+		  * \param[in] f_0 initial function value, \f$ \phi(0) \f$ in Moore-Thuente (1994)
+		  * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994)
+		  * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994]
+		  * \return sufficient decrease value
+		  */
+		inline double
+			auxiliaryFunction_PsiMT(double a, double f_a, double f_0, double g_0, double mu = 1.e-4)
+		{
+			return (f_a - f_0 - mu * g_0 * a);
+		}
+
+		/** \brief Auxiliary function derivative used to determine endpoints of More-Thuente interval.
+		  * \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente 1994)
+		  * \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in More-Thuente (1994)
+		  * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994)
+		  * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994]
+		  * \return sufficient decrease derivative
+		  */
+		inline double
+			auxiliaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4)
+		{
+			return (g_a - mu * g_0);
+		}
+
+		/** \brief The voxel grid generated from target cloud containing point means and covariances. */
+		TargetGrid target_cells_;
+
+		//double fitness_epsilon_;
+
+		/** \brief The side length of voxels. */
+		float resolution_;
+
+		/** \brief The maximum step length. */
+		double step_size_;
+
+		/** \brief The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7 [Magnusson 2009]. */
+		double outlier_ratio_;
+
+		/** \brief The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 [Magnusson 2009]. */
+		double gauss_d1_, gauss_d2_, gauss_d3_;
+
+		/** \brief The probability score of the transform applied to the input cloud, Equation 6.9 and 6.10 [Magnusson 2009]. */
+		double trans_probability_;
+
+		/** \brief Precomputed Angular Gradient
+		  *
+		  * The precomputed angular derivatives for the jacobian of a transformation vector, Equation 6.19 [Magnusson 2009].
+		  */
+		Eigen::Vector3d j_ang_a_, j_ang_b_, j_ang_c_, j_ang_d_, j_ang_e_, j_ang_f_, j_ang_g_, j_ang_h_;
+
+		Eigen::Matrix<float, 8, 4> j_ang;
+
+		/** \brief Precomputed Angular Hessian
+		  *
+		  * The precomputed angular derivatives for the hessian of a transformation vector, Equation 6.19 [Magnusson 2009].
+		  */
+		Eigen::Vector3d h_ang_a2_, h_ang_a3_,
+			h_ang_b2_, h_ang_b3_,
+			h_ang_c2_, h_ang_c3_,
+			h_ang_d1_, h_ang_d2_, h_ang_d3_,
+			h_ang_e1_, h_ang_e2_, h_ang_e3_,
+			h_ang_f1_, h_ang_f2_, h_ang_f3_;
+
+		Eigen::Matrix<float, 16, 4> h_ang;
+
+		/** \brief The first order derivative of the transformation of a point w.r.t. the transform vector, \f$ J_E \f$ in Equation 6.18 [Magnusson 2009]. */
+  //      Eigen::Matrix<double, 3, 6> point_gradient_;
+
+		/** \brief The second order derivative of the transformation of a point w.r.t. the transform vector, \f$ H_E \f$ in Equation 6.20 [Magnusson 2009]. */
+  //      Eigen::Matrix<double, 18, 6> point_hessian_;
+
+    int num_threads_;
+
+	public:
+		NeighborSearchMethod search_method;
+
+		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	};
+
+}
+
+#endif // PCL_REGISTRATION_NDT_H_

src/detection/detection_ndt_pclomp/pclomp/pclomp/voxel_grid_covariance_omp.h

@@ -0,0 +1,557 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_VOXEL_GRID_COVARIANCE_OMP_H_
+#define PCL_VOXEL_GRID_COVARIANCE_OMP_H_
+
+#include <pcl/pcl_macros.h>
+#include <pcl/filters/boost.h>
+#include <pcl/filters/voxel_grid.h>
+#include <map>
+#include <unordered_map>
+#include <pcl/point_types.h>
+#include <pcl/kdtree/kdtree_flann.h>
+
+namespace pclomp
+{
+  /** \brief A searchable voxel structure containing the mean and covariance of the data.
+    * \note For more information please see
+    * <b>Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform —
+    * an Efficient Representation for Registration, Surface Analysis, and Loop Detection.
+    * PhD thesis, Orebro University. Orebro Studies in Technology 36</b>
+    * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
+    */
+  template<typename PointT>
+  class VoxelGridCovariance : public pcl::VoxelGrid<PointT>
+  {
+    protected:
+      using pcl::VoxelGrid<PointT>::filter_name_;
+      using pcl::VoxelGrid<PointT>::getClassName;
+      using pcl::VoxelGrid<PointT>::input_;
+      using pcl::VoxelGrid<PointT>::indices_;
+      using pcl::VoxelGrid<PointT>::filter_limit_negative_;
+      using pcl::VoxelGrid<PointT>::filter_limit_min_;
+      using pcl::VoxelGrid<PointT>::filter_limit_max_;
+      using pcl::VoxelGrid<PointT>::filter_field_name_;
+
+      using pcl::VoxelGrid<PointT>::downsample_all_data_;
+      using pcl::VoxelGrid<PointT>::leaf_layout_;
+      using pcl::VoxelGrid<PointT>::save_leaf_layout_;
+      using pcl::VoxelGrid<PointT>::leaf_size_;
+      using pcl::VoxelGrid<PointT>::min_b_;
+      using pcl::VoxelGrid<PointT>::max_b_;
+      using pcl::VoxelGrid<PointT>::inverse_leaf_size_;
+      using pcl::VoxelGrid<PointT>::div_b_;
+      using pcl::VoxelGrid<PointT>::divb_mul_;
+
+      typedef typename pcl::traits::fieldList<PointT>::type FieldList;
+      typedef typename pcl::Filter<PointT>::PointCloud PointCloud;
+      typedef typename PointCloud::Ptr PointCloudPtr;
+      typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+    public:
+
+#if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
+      typedef pcl::shared_ptr< pcl::VoxelGrid<PointT> > Ptr;
+      typedef pcl::shared_ptr< const pcl::VoxelGrid<PointT> > ConstPtr;
+#else
+      typedef boost::shared_ptr< pcl::VoxelGrid<PointT> > Ptr;
+      typedef boost::shared_ptr< const pcl::VoxelGrid<PointT> > ConstPtr;
+#endif
+
+      /** \brief Simple structure to hold a centroid, covariance and the number of points in a leaf.
+        * Inverse covariance, eigen vectors and eigen values are precomputed. */
+      struct Leaf
+      {
+        /** \brief Constructor.
+         * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref evecs_ to the identity matrix
+         */
+        Leaf () :
+          nr_points (0),
+          mean_ (Eigen::Vector3d::Zero ()),
+          centroid (),
+          cov_ (Eigen::Matrix3d::Identity ()),
+          icov_ (Eigen::Matrix3d::Zero ()),
+          evecs_ (Eigen::Matrix3d::Identity ()),
+          evals_ (Eigen::Vector3d::Zero ())
+        {
+        }
+
+        /** \brief Get the voxel covariance.
+          * \return covariance matrix
+          */
+        Eigen::Matrix3d
+        getCov () const
+        {
+          return (cov_);
+        }
+
+        /** \brief Get the inverse of the voxel covariance.
+          * \return inverse covariance matrix
+          */
+        Eigen::Matrix3d
+        getInverseCov () const
+        {
+          return (icov_);
+        }
+
+        /** \brief Get the voxel centroid.
+          * \return centroid
+          */
+        Eigen::Vector3d
+        getMean () const
+        {
+          return (mean_);
+        }
+
+        /** \brief Get the eigen vectors of the voxel covariance.
+          * \note Order corresponds with \ref getEvals
+          * \return matrix whose columns contain eigen vectors
+          */
+        Eigen::Matrix3d
+        getEvecs () const
+        {
+          return (evecs_);
+        }
+
+        /** \brief Get the eigen values of the voxel covariance.
+          * \note Order corresponds with \ref getEvecs
+          * \return vector of eigen values
+          */
+        Eigen::Vector3d
+        getEvals () const
+        {
+          return (evals_);
+        }
+
+        /** \brief Get the number of points contained by this voxel.
+          * \return number of points
+          */
+        int
+        getPointCount () const
+        {
+          return (nr_points);
+        }
+
+        /** \brief Number of points contained by voxel */
+        int nr_points;
+
+        /** \brief 3D voxel centroid */
+        Eigen::Vector3d mean_;
+
+        /** \brief Nd voxel centroid
+         * \note Differs from \ref mean_ when color data is used
+         */
+        Eigen::VectorXf centroid;
+
+        /** \brief Voxel covariance matrix */
+        Eigen::Matrix3d cov_;
+
+        /** \brief Inverse of voxel covariance matrix */
+        Eigen::Matrix3d icov_;
+
+        /** \brief Eigen vectors of voxel covariance matrix */
+        Eigen::Matrix3d evecs_;
+
+        /** \brief Eigen values of voxel covariance matrix */
+        Eigen::Vector3d evals_;
+
+      };
+
+      /** \brief Pointer to VoxelGridCovariance leaf structure */
+      typedef Leaf* LeafPtr;
+
+      /** \brief Const pointer to VoxelGridCovariance leaf structure */
+      typedef const Leaf* LeafConstPtr;
+
+    typedef std::map<size_t, Leaf> Map;
+
+    public:
+
+      /** \brief Constructor.
+       * Sets \ref leaf_size_ to 0 and \ref searchable_ to false.
+       */
+      VoxelGridCovariance () :
+        searchable_ (true),
+        min_points_per_voxel_ (6),
+        min_covar_eigvalue_mult_ (0.01),
+        leaves_ (),
+        voxel_centroids_ (),
+        voxel_centroids_leaf_indices_ (),
+        kdtree_ ()
+      {
+        downsample_all_data_ = false;
+        save_leaf_layout_ = false;
+        leaf_size_.setZero ();
+        min_b_.setZero ();
+        max_b_.setZero ();
+        filter_name_ = "VoxelGridCovariance";
+      }
+
+      /** \brief Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance calculation).
+        * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used
+        */
+      inline void
+      setMinPointPerVoxel (int min_points_per_voxel)
+      {
+        if(min_points_per_voxel > 2)
+        {
+          min_points_per_voxel_ = min_points_per_voxel;
+        }
+        else
+        {
+          PCL_WARN ("%s: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", this->getClassName ().c_str ());
+          min_points_per_voxel_ = 3;
+        }
+      }
+
+      /** \brief Get the minimum number of points required for a cell to be used.
+        * \return the minimum number of points for required for a voxel to be used
+        */
+      inline int
+      getMinPointPerVoxel ()
+      {
+        return min_points_per_voxel_;
+      }
+
+      /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
+        * \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues
+        */
+      inline void
+      setCovEigValueInflationRatio (double min_covar_eigvalue_mult)
+      {
+        min_covar_eigvalue_mult_ = min_covar_eigvalue_mult;
+      }
+
+      /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
+        * \return the minimum allowable ratio between eigenvalues
+        */
+      inline double
+      getCovEigValueInflationRatio ()
+      {
+        return min_covar_eigvalue_mult_;
+      }
+
+      /** \brief Filter cloud and initializes voxel structure.
+       * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
+       * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
+       */
+      inline void
+      filter (PointCloud &output, bool searchable = false)
+      {
+        searchable_ = searchable;
+        applyFilter (output);
+
+        voxel_centroids_ = PointCloudPtr (new PointCloud (output));
+
+        if (searchable_ && voxel_centroids_->size() > 0)
+        {
+          // Initiates kdtree of the centroids of voxels containing a sufficient number of points
+          kdtree_.setInputCloud (voxel_centroids_);
+        }
+      }
+
+      /** \brief Initializes voxel structure.
+       * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
+       */
+      inline void
+      filter (bool searchable = false)
+      {
+        searchable_ = searchable;
+        voxel_centroids_ = PointCloudPtr (new PointCloud);
+        applyFilter (*voxel_centroids_);
+
+        if (searchable_ && voxel_centroids_->size() > 0)
+        {
+          // Initiates kdtree of the centroids of voxels containing a sufficient number of points
+          kdtree_.setInputCloud (voxel_centroids_);
+        }
+      }
+
+      /** \brief Get the voxel containing point p.
+       * \param[in] index the index of the leaf structure node
+       * \return const pointer to leaf structure
+       */
+      inline LeafConstPtr
+      getLeaf (int index)
+      {
+        auto leaf_iter = leaves_.find (index);
+        if (leaf_iter != leaves_.end ())
+        {
+          LeafConstPtr ret (&(leaf_iter->second));
+          return ret;
+        }
+        else
+          return NULL;
+      }
+
+      /** \brief Get the voxel containing point p.
+       * \param[in] p the point to get the leaf structure at
+       * \return const pointer to leaf structure
+       */
+      inline LeafConstPtr
+      getLeaf (PointT &p)
+      {
+        // Generate index associated with p
+        int ijk0 = static_cast<int> (floor (p.x * inverse_leaf_size_[0]) - min_b_[0]);
+        int ijk1 = static_cast<int> (floor (p.y * inverse_leaf_size_[1]) - min_b_[1]);
+        int ijk2 = static_cast<int> (floor (p.z * inverse_leaf_size_[2]) - min_b_[2]);
+
+        // Compute the centroid leaf index
+        int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
+
+        // Find leaf associated with index
+        auto leaf_iter = leaves_.find (idx);
+        if (leaf_iter != leaves_.end ())
+        {
+          // If such a leaf exists return the pointer to the leaf structure
+          LeafConstPtr ret (&(leaf_iter->second));
+          return ret;
+        }
+        else
+          return NULL;
+      }
+
+      /** \brief Get the voxel containing point p.
+       * \param[in] p the point to get the leaf structure at
+       * \return const pointer to leaf structure
+       */
+      inline LeafConstPtr
+      getLeaf (Eigen::Vector3f &p)
+      {
+        // Generate index associated with p
+        int ijk0 = static_cast<int> (floor (p[0] * inverse_leaf_size_[0]) - min_b_[0]);
+        int ijk1 = static_cast<int> (floor (p[1] * inverse_leaf_size_[1]) - min_b_[1]);
+        int ijk2 = static_cast<int> (floor (p[2] * inverse_leaf_size_[2]) - min_b_[2]);
+
+        // Compute the centroid leaf index
+        int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
+
+        // Find leaf associated with index
+        auto leaf_iter = leaves_.find (idx);
+        if (leaf_iter != leaves_.end ())
+        {
+          // If such a leaf exists return the pointer to the leaf structure
+          LeafConstPtr ret (&(leaf_iter->second));
+          return ret;
+        }
+        else
+          return NULL;
+
+      }
+
+      /** \brief Get the voxels surrounding point p, not including the voxel containing point p.
+       * \note Only voxels containing a sufficient number of points are used (slower than radius search in practice).
+       * \param[in] reference_point the point to get the leaf structure at
+       * \param[out] neighbors
+       * \return number of neighbors found
+       */
+	  int getNeighborhoodAtPoint(const Eigen::MatrixXi&, const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const ;
+	  int getNeighborhoodAtPoint(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const ;
+	  int getNeighborhoodAtPoint7(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const ;
+	  int getNeighborhoodAtPoint1(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const ;
+
+      /** \brief Get the leaf structure map
+       * \return a map containing all leaves
+       */
+      inline const Map&
+      getLeaves ()
+      {
+        return leaves_;
+      }
+
+      /** \brief Get a pointcloud containing the voxel centroids
+       * \note Only voxels containing a sufficient number of points are used.
+       * \return a map containing all leaves
+       */
+      inline PointCloudPtr
+      getCentroids ()
+      {
+        return voxel_centroids_;
+      }
+
+
+      /** \brief Get a cloud to visualize each voxels normal distribution.
+       * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel
+       */
+      void
+      getDisplayCloud (pcl::PointCloud<pcl::PointXYZ>& cell_cloud);
+
+      /** \brief Search for the k-nearest occupied voxels for the given query point.
+       * \note Only voxels containing a sufficient number of points are used.
+       * \param[in] point the given query point
+       * \param[in] k the number of neighbors to search for
+       * \param[out] k_leaves the resultant leaves of the neighboring points
+       * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
+       * \return number of neighbors found
+       */
+      int
+      nearestKSearch (const PointT &point, int k,
+                      std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
+      {
+        k_leaves.clear ();
+
+        // Check if kdtree has been built
+        if (!searchable_)
+        {
+          PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
+          return 0;
+        }
+
+        // Find k-nearest neighbors in the occupied voxel centroid cloud
+        std::vector<int> k_indices;
+        k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances);
+
+        // Find leaves corresponding to neighbors
+        k_leaves.reserve (k);
+        for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++)
+        {
+          k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[*iter]]);
+        }
+        return k;
+      }
+
+      /** \brief Search for the k-nearest occupied voxels for the given query point.
+       * \note Only voxels containing a sufficient number of points are used.
+       * \param[in] cloud the given query point
+       * \param[in] index the index
+       * \param[in] k the number of neighbors to search for
+       * \param[out] k_leaves the resultant leaves of the neighboring points
+       * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
+       * \return number of neighbors found
+       */
+      inline int
+      nearestKSearch (const PointCloud &cloud, int index, int k,
+                      std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
+      {
+        if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
+          return (0);
+        return (nearestKSearch (cloud.points[index], k, k_leaves, k_sqr_distances));
+      }
+
+
+      /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
+       * \note Only voxels containing a sufficient number of points are used.
+       * \param[in] point the given query point
+       * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
+       * \param[out] k_leaves the resultant leaves of the neighboring points
+       * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
+       * \param[in] max_nn
+       * \return number of neighbors found
+       */
+      int
+      radiusSearch (const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves,
+                    std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
+      {
+        k_leaves.clear ();
+
+        // Check if kdtree has been built
+        if (!searchable_)
+        {
+          PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
+          return 0;
+        }
+
+        // Find neighbors within radius in the occupied voxel centroid cloud
+        std::vector<int> k_indices;
+        int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
+
+        // Find leaves corresponding to neighbors
+        k_leaves.reserve (k);
+        for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++)
+        {
+		  auto leaf = leaves_.find(voxel_centroids_leaf_indices_[*iter]);
+		  if (leaf == leaves_.end()) {
+			  std::cerr << "error : could not find the leaf corresponding to the voxel" << std::endl;
+			  std::cin.ignore(1);
+		  }
+          k_leaves.push_back (&(leaf->second));
+        }
+        return k;
+      }
+
+      /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
+       * \note Only voxels containing a sufficient number of points are used.
+       * \param[in] cloud the given query point
+       * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point
+       * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
+       * \param[out] k_leaves the resultant leaves of the neighboring points
+       * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
+       * \param[in] max_nn
+       * \return number of neighbors found
+       */
+      inline int
+      radiusSearch (const PointCloud &cloud, int index, double radius,
+                    std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
+                    unsigned int max_nn = 0) const
+      {
+        if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
+          return (0);
+        return (radiusSearch (cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn));
+      }
+
+    protected:
+
+      /** \brief Filter cloud and initializes voxel structure.
+       * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
+       */
+      void applyFilter (PointCloud &output);
+
+      /** \brief Flag to determine if voxel structure is searchable. */
+      bool searchable_;
+
+      /** \brief Minimum points contained with in a voxel to allow it to be usable. */
+      int min_points_per_voxel_;
+
+      /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */
+      double min_covar_eigvalue_mult_;
+
+      /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */
+	  Map leaves_;
+
+      /** \brief Point cloud containing centroids of voxels containing at least minimum number of points. */
+      PointCloudPtr voxel_centroids_;
+
+      /** \brief Indices of leaf structures associated with each point in \ref voxel_centroids_ (used for searching). */
+      std::vector<int> voxel_centroids_leaf_indices_;
+
+      /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */
+      pcl::KdTreeFLANN<PointT> kdtree_;
+  };
+}
+
+#endif  //#ifndef PCL_VOXEL_GRID_COVARIANCE_H_

src/detection/detection_ndt_pclomp/pclomp/voxel_grid_covariance_omp.cpp

@@ -0,0 +1,5 @@
+#include <pclomp/voxel_grid_covariance_omp.h>
+#include <voxel_grid_covariance_omp_impl.hpp>
+
+template class pclomp::VoxelGridCovariance<pcl::PointXYZ>;
+template class pclomp::VoxelGridCovariance<pcl::PointXYZI>;

src/detection/detection_ndt_pclomp/pclomp/voxel_grid_covariance_omp_impl.hpp

@@ -0,0 +1,487 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_
+#define PCL_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_
+
+#include <pcl/common/common.h>
+#include <pcl/filters/boost.h>
+#include <pclomp/voxel_grid_covariance_omp.h>
+#include <Eigen/Dense>
+#include <Eigen/Cholesky>
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> void
+pclomp::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output)
+{
+  voxel_centroids_leaf_indices_.clear ();
+
+  // Has the input dataset been set already?
+  if (!input_)
+  {
+    PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
+    output.width = output.height = 0;
+    output.points.clear ();
+    return;
+  }
+
+  // Copy the header (and thus the frame_id) + allocate enough space for points
+  output.height = 1;                          // downsampling breaks the organized structure
+  output.is_dense = true;                     // we filter out invalid points
+  output.points.clear ();
+
+  Eigen::Vector4f min_p, max_p;
+  // Get the minimum and maximum dimensions
+  if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud...
+      pcl::getMinMax3D<PointT> (input_, filter_field_name_, static_cast<float> (filter_limit_min_), static_cast<float> (filter_limit_max_), min_p, max_p, filter_limit_negative_);
+  else
+	  pcl::getMinMax3D<PointT> (*input_, min_p, max_p);
+
+  // Check that the leaf size is not too small, given the size of the data
+  int64_t dx = static_cast<int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
+  int64_t dy = static_cast<int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
+  int64_t dz = static_cast<int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
+
+  if((dx*dy*dz) > std::numeric_limits<int32_t>::max())
+  {
+    PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
+    output.clear();
+    return;
+  }
+
+  // Compute the minimum and maximum bounding box values
+  min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0]));
+  max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
+  min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
+  max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
+  min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
+  max_b_[2] = static_cast<int> (floor (max_p[2] * inverse_leaf_size_[2]));
+
+  // Compute the number of divisions needed along all axis
+  div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
+  div_b_[3] = 0;
+
+  // Clear the leaves
+  leaves_.clear ();
+//  leaves_.reserve(8192);
+
+  // Set up the division multiplier
+  divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
+
+  int centroid_size = 4;
+
+  if (downsample_all_data_)
+    centroid_size = boost::mpl::size<FieldList>::value;
+
+  // ---[ RGB special case
+  std::vector<pcl::PCLPointField> fields;
+  int rgba_index = -1;
+  rgba_index = pcl::getFieldIndex<PointT> ("rgb", fields);
+  if (rgba_index == -1)
+    rgba_index = pcl::getFieldIndex<PointT> ("rgba", fields);
+  if (rgba_index >= 0)
+  {
+    rgba_index = fields[rgba_index].offset;
+    centroid_size += 3;
+  }
+
+  // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
+  if (!filter_field_name_.empty ())
+  {
+    // Get the distance field index
+    std::vector<pcl::PCLPointField> fields;
+    int distance_idx = pcl::getFieldIndex<PointT> (filter_field_name_, fields);
+    if (distance_idx == -1)
+      PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
+
+    // First pass: go over all points and insert them into the right leaf
+    for (size_t cp = 0; cp < input_->points.size (); ++cp)
+    {
+      if (!input_->is_dense)
+        // Check if the point is invalid
+        if (!std::isfinite (input_->points[cp].x) ||
+            !std::isfinite (input_->points[cp].y) ||
+            !std::isfinite (input_->points[cp].z))
+          continue;
+
+      // Get the distance value
+      const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&input_->points[cp]);
+      float distance_value = 0;
+      memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
+
+      if (filter_limit_negative_)
+      {
+        // Use a threshold for cutting out points which inside the interval
+        if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
+          continue;
+      }
+      else
+      {
+        // Use a threshold for cutting out points which are too close/far away
+        if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
+          continue;
+      }
+
+      int ijk0 = static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
+      int ijk1 = static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
+      int ijk2 = static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
+
+      // Compute the centroid leaf index
+      int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
+
+      Leaf& leaf = leaves_[idx];
+      if (leaf.nr_points == 0)
+      {
+        leaf.centroid.resize (centroid_size);
+        leaf.centroid.setZero ();
+      }
+
+      Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z);
+      // Accumulate point sum for centroid calculation
+      leaf.mean_ += pt3d;
+      // Accumulate x*xT for single pass covariance calculation
+      leaf.cov_ += pt3d * pt3d.transpose ();
+
+      // Do we need to process all the fields?
+      if (!downsample_all_data_)
+      {
+        Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0);
+        leaf.centroid.template head<4> () += pt;
+      }
+      else
+      {
+        // Copy all the fields
+        Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
+        // ---[ RGB special case
+        if (rgba_index >= 0)
+        {
+          // fill r/g/b data
+          int rgb;
+          memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (int));
+          centroid[centroid_size - 3] = static_cast<float> ((rgb >> 16) & 0x0000ff);
+          centroid[centroid_size - 2] = static_cast<float> ((rgb >> 8) & 0x0000ff);
+          centroid[centroid_size - 1] = static_cast<float> ((rgb) & 0x0000ff);
+        }
+        pcl::for_each_type<FieldList> (pcl::NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
+        leaf.centroid += centroid;
+      }
+      ++leaf.nr_points;
+    }
+  }
+  // No distance filtering, process all data
+  else
+  {
+    // First pass: go over all points and insert them into the right leaf
+    for (size_t cp = 0; cp < input_->points.size (); ++cp)
+    {
+      if (!input_->is_dense)
+        // Check if the point is invalid
+        if (!std::isfinite (input_->points[cp].x) ||
+            !std::isfinite (input_->points[cp].y) ||
+            !std::isfinite (input_->points[cp].z))
+          continue;
+
+      int ijk0 = static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
+      int ijk1 = static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
+      int ijk2 = static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
+
+      // Compute the centroid leaf index
+      int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
+
+      //int idx = (((input_->points[cp].getArray4fmap () * inverse_leaf_size_).template cast<int> ()).matrix () - min_b_).dot (divb_mul_);
+      Leaf& leaf = leaves_[idx];
+      if (leaf.nr_points == 0)
+      {
+        leaf.centroid.resize (centroid_size);
+        leaf.centroid.setZero ();
+      }
+
+      Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z);
+      // Accumulate point sum for centroid calculation
+      leaf.mean_ += pt3d;
+      // Accumulate x*xT for single pass covariance calculation
+      leaf.cov_ += pt3d * pt3d.transpose ();
+
+      // Do we need to process all the fields?
+      if (!downsample_all_data_)
+      {
+        Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0);
+        leaf.centroid.template head<4> () += pt;
+      }
+      else
+      {
+        // Copy all the fields
+        Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
+        // ---[ RGB special case
+        if (rgba_index >= 0)
+        {
+          // Fill r/g/b data, assuming that the order is BGRA
+          int rgb;
+          memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (int));
+          centroid[centroid_size - 3] = static_cast<float> ((rgb >> 16) & 0x0000ff);
+          centroid[centroid_size - 2] = static_cast<float> ((rgb >> 8) & 0x0000ff);
+          centroid[centroid_size - 1] = static_cast<float> ((rgb) & 0x0000ff);
+        }
+        pcl::for_each_type<FieldList> (pcl::NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
+        leaf.centroid += centroid;
+      }
+      ++leaf.nr_points;
+    }
+  }
+
+  // Second pass: go over all leaves and compute centroids and covariance matrices
+  output.points.reserve (leaves_.size ());
+  if (searchable_)
+    voxel_centroids_leaf_indices_.reserve (leaves_.size ());
+  int cp = 0;
+  if (save_leaf_layout_)
+    leaf_layout_.resize (div_b_[0] * div_b_[1] * div_b_[2], -1);
+
+  // Eigen values and vectors calculated to prevent near singular matrices
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver;
+  Eigen::Matrix3d eigen_val;
+  Eigen::Vector3d pt_sum;
+
+  // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the max eigen value.
+  double min_covar_eigvalue;
+
+  for (auto it = leaves_.begin (); it != leaves_.end (); ++it)
+  {
+
+    // Normalize the centroid
+    Leaf& leaf = it->second;
+
+    // Normalize the centroid
+    leaf.centroid /= static_cast<float> (leaf.nr_points);
+    // Point sum used for single pass covariance calculation
+    pt_sum = leaf.mean_;
+    // Normalize mean
+    leaf.mean_ /= leaf.nr_points;
+
+    // If the voxel contains sufficient points, its covariance is calculated and is added to the voxel centroids and output clouds.
+    // Points with less than the minimum points will have a can not be accurately approximated using a normal distribution.
+    if (leaf.nr_points >= min_points_per_voxel_)
+    {
+      if (save_leaf_layout_)
+        leaf_layout_[it->first] = cp++;
+
+      output.push_back (PointT ());
+
+      // Do we need to process all the fields?
+      if (!downsample_all_data_)
+      {
+        output.points.back ().x = leaf.centroid[0];
+        output.points.back ().y = leaf.centroid[1];
+        output.points.back ().z = leaf.centroid[2];
+      }
+      else
+      {
+        pcl::for_each_type<FieldList> (pcl::NdCopyEigenPointFunctor<PointT> (leaf.centroid, output.back ()));
+        // ---[ RGB special case
+        if (rgba_index >= 0)
+        {
+          // pack r/g/b into rgb
+          float r = leaf.centroid[centroid_size - 3], g = leaf.centroid[centroid_size - 2], b = leaf.centroid[centroid_size - 1];
+          int rgb = (static_cast<int> (r)) << 16 | (static_cast<int> (g)) << 8 | (static_cast<int> (b));
+          memcpy (reinterpret_cast<char*> (&output.points.back ()) + rgba_index, &rgb, sizeof (float));
+        }
+      }
+
+      // Stores the voxel indices for fast access searching
+      if (searchable_)
+        voxel_centroids_leaf_indices_.push_back (static_cast<int> (it->first));
+
+      // Single pass covariance calculation
+      leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose ())) / leaf.nr_points + leaf.mean_ * leaf.mean_.transpose ();
+      leaf.cov_ *= (leaf.nr_points - 1.0) / leaf.nr_points;
+
+      //Normalize Eigen Val such that max no more than 100x min.
+      eigensolver.compute (leaf.cov_);
+      eigen_val = eigensolver.eigenvalues ().asDiagonal ();
+      leaf.evecs_ = eigensolver.eigenvectors ();
+
+      if (eigen_val (0, 0) < 0 || eigen_val (1, 1) < 0 || eigen_val (2, 2) <= 0)
+      {
+        leaf.nr_points = -1;
+        continue;
+      }
+
+      // Avoids matrices near singularities (eq 6.11)[Magnusson 2009]
+
+      min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val (2, 2);
+      if (eigen_val (0, 0) < min_covar_eigvalue)
+      {
+        eigen_val (0, 0) = min_covar_eigvalue;
+
+        if (eigen_val (1, 1) < min_covar_eigvalue)
+        {
+          eigen_val (1, 1) = min_covar_eigvalue;
+        }
+
+        leaf.cov_ = leaf.evecs_ * eigen_val * leaf.evecs_.inverse ();
+      }
+      leaf.evals_ = eigen_val.diagonal ();
+
+      leaf.icov_ = leaf.cov_.inverse ();
+      if (leaf.icov_.maxCoeff () == std::numeric_limits<float>::infinity ( )
+          || leaf.icov_.minCoeff () == -std::numeric_limits<float>::infinity ( ) )
+      {
+        leaf.nr_points = -1;
+      }
+
+    }
+  }
+
+  output.width = static_cast<uint32_t> (output.points.size ());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> int
+pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint(const Eigen::MatrixXi& relative_coordinates, const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
+{
+	neighbors.clear();
+
+	// Find displacement coordinates
+	Eigen::Vector4i ijk(static_cast<int> (floor(reference_point.x / leaf_size_[0])),
+		static_cast<int> (floor(reference_point.y / leaf_size_[1])),
+		static_cast<int> (floor(reference_point.z / leaf_size_[2])), 0);
+	Eigen::Array4i diff2min = min_b_ - ijk;
+	Eigen::Array4i diff2max = max_b_ - ijk;
+	neighbors.reserve(relative_coordinates.cols());
+
+	// Check each neighbor to see if it is occupied and contains sufficient points
+	// Slower than radius search because needs to check 26 indices
+	for (int ni = 0; ni < relative_coordinates.cols(); ni++)
+	{
+		Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
+		// Checking if the specified cell is in the grid
+		if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
+		{
+			auto leaf_iter = leaves_.find(((ijk + displacement - min_b_).dot(divb_mul_)));
+			if (leaf_iter != leaves_.end() && leaf_iter->second.nr_points >= min_points_per_voxel_)
+			{
+				LeafConstPtr leaf = &(leaf_iter->second);
+				neighbors.push_back(leaf);
+			}
+		}
+	}
+
+	return (static_cast<int> (neighbors.size()));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> int
+pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
+{
+	neighbors.clear();
+
+	// Find displacement coordinates
+	Eigen::MatrixXi relative_coordinates = pcl::getAllNeighborCellIndices();
+	return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> int
+pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint7(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
+{
+	neighbors.clear();
+
+	Eigen::MatrixXi relative_coordinates(3, 7);
+	relative_coordinates.setZero();
+	relative_coordinates(0, 1) = 1;
+	relative_coordinates(0, 2) = -1;
+	relative_coordinates(1, 3) = 1;
+	relative_coordinates(1, 4) = -1;
+	relative_coordinates(2, 5) = 1;
+	relative_coordinates(2, 6) = -1;
+
+	return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> int
+pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint1(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
+{
+	neighbors.clear();
+	return getNeighborhoodAtPoint(Eigen::MatrixXi::Zero(3,1), reference_point, neighbors);
+}
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> void
+pclomp::VoxelGridCovariance<PointT>::getDisplayCloud (pcl::PointCloud<pcl::PointXYZ>& cell_cloud)
+{
+  cell_cloud.clear ();
+
+  int pnt_per_cell = 1000;
+  boost::mt19937 rng;
+  boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ());
+  boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
+
+  Eigen::LLT<Eigen::Matrix3d> llt_of_cov;
+  Eigen::Matrix3d cholesky_decomp;
+  Eigen::Vector3d cell_mean;
+  Eigen::Vector3d rand_point;
+  Eigen::Vector3d dist_point;
+
+  // Generate points for each occupied voxel with sufficient points.
+  for (auto it = leaves_.begin (); it != leaves_.end (); ++it)
+  {
+    Leaf& leaf = it->second;
+
+    if (leaf.nr_points >= min_points_per_voxel_)
+    {
+      cell_mean = leaf.mean_;
+      llt_of_cov.compute (leaf.cov_);
+      cholesky_decomp = llt_of_cov.matrixL ();
+
+      // Random points generated by sampling the normal distribution given by voxel mean and covariance matrix
+      for (int i = 0; i < pnt_per_cell; i++)
+      {
+        rand_point = Eigen::Vector3d (var_nor (), var_nor (), var_nor ());
+        dist_point = cell_mean + cholesky_decomp * rand_point;
+        cell_cloud.push_back (pcl::PointXYZ (static_cast<float> (dist_point (0)), static_cast<float> (dist_point (1)), static_cast<float> (dist_point (2))));
+      }
+    }
+  }
+}
+
+#define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance<T>;
+
+#endif    // PCL_VOXEL_GRID_COVARIANCE_IMPL_H_

src/driver/driver_cloud_swap_client/grpcclient.cpp

@@ -17,7 +17,7 @@ grpcclient::grpcclient(std::string stryamlpath)
     ggrpcclient = this;
     dec_yaml(stryamlpath.data());
 
-    mpCalcPing = new CalcPing(gstrserverip,5);
+
 
     int i;
     for(i=0;i<(int)mvectormsgunit.size();i++)
@@ -259,6 +259,7 @@ void grpcclient::threadRPC(std::shared_ptr<qint64> pnrpctime,std::shared_ptr<boo
 
 void grpcclient::run()
 {
+    mpCalcPing = new CalcPing(gstrserverip,5);
             std::shared_ptr<bool> pbRun(new bool);
             *pbRun = true;
                     std::shared_ptr<qint64> pntime(new qint64);
@@ -387,7 +388,7 @@ void grpcclient::shareswapmsg(iv::cloud::cloudmsg * pxmsg)
     for(i=0;i<nsize;i++)
     {
         int j;
-        int nquerysize = mvectorctrlmsgunit.size();
+        int nquerysize = static_cast<int>(mvectorctrlmsgunit.size());
         for(j=0;j<nquerysize;j++)
         {
             if(strncmp(pxmsg->xclouddata(i).msgname().data(), mvectorctrlmsgunit[j].mstrmsgname,255) == 0)
@@ -397,7 +398,31 @@ void grpcclient::shareswapmsg(iv::cloud::cloudmsg * pxmsg)
                 break;
             }
         }
+
+        if((mnShareMode == Mode_Inter) &&(pxmsg->xclouddata(i).data().size()>0))
+        {
+            iv::swapmsgbuff xswapbuff;
+            xswapbuff.mstrmsgname = pxmsg->xclouddata(i).msgname();
+            xswapbuff.mndatalen = pxmsg->xclouddata(i).data().size();
+            xswapbuff.mpdata_ptr = std::shared_ptr<char>(new char[xswapbuff.mndatalen]);
+            memcpy(xswapbuff.mpdata_ptr.get(),pxmsg->xclouddata(i).data().data(),xswapbuff.mndatalen);
+            if(mvectorswapbuff.size() < 10000)
+            {
+                mmutexswapbuff.lock();
+                mvectorswapbuff.push_back(xswapbuff);
+                mmutexswapbuff.unlock();
+
+                mcv_swapbuff.notify_all();
+            }
+            else
+            {
+                std::cout<<" swap buff full. "<<std::endl;
+            }
+
+        }
     }
+
+
 }
 
 void grpcclient::UpdateData(const char *strdata, const unsigned int nSize, const char *strmemname)
@@ -414,3 +439,58 @@ void grpcclient::UpdateData(const char *strdata, const unsigned int nSize, const
     mmutexmsg.unlock();
 
 }
+
+void grpcclient::SetServerPort(std::string strServer,std::string strPort)
+{
+    gstrserverip = strServer;
+    gstrserverport = strPort;
+}
+
+void grpcclient::SetNodeId(std::string strnodeid,std::string strobjnodeid)
+{
+    mstrnodeid = strnodeid;
+    mstrobjnodeid = strobjnodeid;
+}
+
+void grpcclient::SetShareMode(ShareMode xMode)
+{
+    mnShareMode = xMode;
+}
+
+int grpcclient::GetSwapBuffData(iv::swapmsgbuff & xswap,int nwaitms)
+{
+    int nrtn = 0;
+    mmutexswapbuff.lock();
+    if(mvectorswapbuff.size()>0)
+    {
+        xswap = mvectorswapbuff[0];
+        mvectorswapbuff.erase(mvectorswapbuff.begin());
+        nrtn = 1;
+    }
+    mmutexswapbuff.unlock();
+
+    if(nrtn == 1)return  nrtn;
+
+    if(nwaitms == 0)return 0;
+
+    std::unique_lock<std::mutex> lk(mcvmutex_swapbuff);
+    if(mcv_swapbuff.wait_for(lk, std::chrono::milliseconds(nwaitms)) == std::cv_status::timeout)
+    {
+        lk.unlock();
+    }
+    else
+    {
+        lk.unlock();
+    }
+
+    mmutexswapbuff.lock();
+    if(mvectorswapbuff.size()>0)
+    {
+        xswap = mvectorswapbuff[0];
+        mvectorswapbuff.erase(mvectorswapbuff.begin());
+        nrtn = 1;
+    }
+    mmutexswapbuff.unlock();
+
+    return nrtn;
+}

src/driver/driver_cloud_swap_client/grpcclient.h

@@ -31,6 +31,8 @@
 
 #include "cloudswap.grpc.pb.h"
 
+#include <condition_variable>
+
 
 namespace iv {
 struct msgunit
@@ -45,6 +47,14 @@ struct msgunit
     bool mbImportant = false;
     int mnkeeptime = 100;
 };
+
+struct swapmsgbuff
+{
+    std::string mstrmsgname;
+    std::shared_ptr<char> mpdata_ptr;
+    unsigned int mndatalen;
+};
+
 }
 
 
@@ -54,9 +64,17 @@ using grpc::Status;
 
 class grpcclient : public QThread
 {
+
 public:
     grpcclient(std::string stryamlpath);
 
+public:
+    enum ShareMode
+    {
+        Mode_ModuleShare,
+        Mode_Inter
+    };
+
 private:
     std::string gstrserverip =  "0.0.0.0";//"123.57.212.138";
     std::string gstrserverport = "50061";//"9000";
@@ -88,12 +106,29 @@ private:
 
     CalcPing * mpCalcPing;
 
+
+
+    ShareMode mnShareMode  = Mode_ModuleShare;
+
+    std::vector<iv::swapmsgbuff> mvectorswapbuff;
+    std::mutex mmutexswapbuff;
+    std::condition_variable mcv_swapbuff;
+    std::mutex mcvmutex_swapbuff;
+
 private:
     iv::cloud::cloudmsg mcloudemsg;
     std::mutex mmutexmsg;
     void sendcloudmsg(iv::cloud::cloudmsg & xmsg,std::shared_ptr<::grpc::ClientReaderWriter<iv::CloudSwapRequestStream, iv::CloudSwapReplyStream> > writer);
 public:
     void UpdateData(const char * strdata,const unsigned int nSize,const char * strmemname);
+
+    void SetServerPort(std::string strServer,std::string strPort);
+    void SetNodeId(std::string strnodeid,std::string strobjnodeid);
+
+    void SetShareMode(ShareMode xMode);
+
+    int GetSwapBuffData(iv::swapmsgbuff & xswap,int nwaitms = 0);
+
 private:
     void run();
     void dec_yaml(const char * stryamlpath);

src/driver/driver_cloud_swap_server/main.cpp

@@ -1,5 +1,7 @@
 #include <QCoreApplication>
 
+#include <QDateTime>
+
 #include <iostream>
 #include <vector>
 
@@ -110,17 +112,19 @@ class CloudSwapServiceImpl final : public iv::CloudSwapStream::Service {
               std::shared_ptr<char> pdata_ptr = std::shared_ptr<char>(new char[ndatasize]);
               memcpy(pdata_ptr.get(),request.xdata().data(),ndatasize);
               gpswapserver->broadmsg(strobjnodeid,pdata_ptr,ndatasize,request.pingavg(),request.flatencyinstore());
+              std::cout<<QDateTime::currentDateTime().toString("yyyy-MM-dd-hh-mm-ss-zzz:").toStdString()<<"receive data from: "<<strnodeid<<std::endl;
           }
           else
           {
-              std::cout<<"receive heartbeat from "<<strnodeid<<std::endl;
+              std::cout<<QDateTime::currentDateTime().toString("yyyy-MM-dd-hh-mm-ss-zzz:").toStdString()<<"receive heartbeat from "<<strnodeid<<std::endl;
           }
 
 
       }
+      if(pswap == NULL)return Status::OK;
       pswap->stopswap();  //Because connection fail.
-      std::cout<<" no conn"<<std::endl;
-      std::cout<<"dis connect."<<std::endl;
+      std::cout<<QDateTime::currentDateTime().toString("yyyy-MM-dd-hh-mm-ss-zzz:").toStdString()<<" no conn"<<std::endl;
+      std::cout<<QDateTime::currentDateTime().toString("yyyy-MM-dd-hh-mm-ss-zzz:").toStdString()<<"dis connect."<<std::endl;
 
     return Status::OK;
   }
@@ -158,12 +162,283 @@ void RunServer() {
 }
 
 
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <string.h>
+#include <signal.h>
+#include <execinfo.h>
+
+#include <QDateTime>
+#include <QDir>
+
+void out_stack(char *sig);
+void signal_exit(int dunno)
+{
+    char* signal_str =  (char*)" ";
+    char dunno_str[10] = {0};
+    sprintf(dunno_str, "%d", dunno);
+    switch (dunno)
+    {
+        case 1:
+            signal_str =  (char*)"SIGHUP(1)";
+            break;
+        case 2:
+            signal_str =  (char*)"SIGINT(2:CTRL_C)"; //CTRL_C
+            break;
+        case 3:
+            signal_str =  (char*)"SIGQUIT(3)";
+            break;
+        case 6:
+        {
+            signal_str =  (char*)"SIGABRT(6)";
+            out_stack(signal_str);
+        }
+        break;
+        case 9:
+            signal_str =  (char*)"SIGKILL(9)";
+            break;
+        case 15:
+            signal_str =  (char*)"SIGTERM(15 KILL)"; //kill
+            break;
+        case 11:
+        {
+            signal_str =  (char*)"SIGSEGV(11)"; //SIGSEGV
+            out_stack(signal_str);
+        }
+        break;
+        default:
+            signal_str =  (char*)"OTHER";
+            break;
+    }
+    exit(0);
+
+}
+
+
+size_t get_executable_path( char* processdir,char* processname, size_t len)
+{
+        char* path_end;
+        int nsize;
+        if((nsize =readlink("/proc/self/exe", processdir,len)) <=0)
+                return -1;
+
+        processdir[nsize] = '\0';
+        path_end = strrchr(processdir,  '/');
+        if(path_end == NULL)
+                return -1;
+        ++path_end;
+
+
+        strcpy(processname, path_end);
+        *path_end = '\0';
+
+        return (size_t)(path_end - processdir);
+}
+
+void savetofile(char * stroutput)
+{
+    qDebug("save file");
+    char strpath[1024];
+    QDateTime dt = QDateTime::currentDateTime();
+    char path[1000];
+    char processname[1024];
+    get_executable_path(path, processname, 1000);
+    snprintf(strpath,255,"%s/log/%s-%d-%s.log",getenv("HOME"),processname, getpid(),dt.toString("yyyyMMddhhmmsszzz").toLatin1().data());
+
+
+    char strdir[1024];
+    snprintf(strdir,255,"%s/log",getenv("HOME"));
+    QDir xdir;
+    xdir.setPath(strdir);
+    if(!xdir.exists())
+    {
+        qDebug("create dir %s",strdir);
+        xdir.mkdir(strdir);
+    }
+
+    qDebug("%s\n",strpath);
+    FILE* file = fopen(strpath,"w+");
+
+    if(file != nullptr)
+    {
+        fwrite(stroutput,strnlen(stroutput,6000),1,file);
+    }
+
+
+    fclose(file);
+}
+
+static void output_addrline(char addr[],char * strtem)
+{
+    char cmd[256];
+    char line[256];
+    char addrline[32]={0,};
+    char *str1, *str2;
+    FILE* file;
+
+    snprintf(strtem,1000," ");
+
+//    str1 = strchr(addr,'[');
+//    str2 = strchr(addr, ']');
+
+    str1 = strchr(addr,'(');
+    str2 = strchr(addr, ')');
+    if(str1 == NULL || str2 == NULL)
+    {
+        return;
+    }
+
+    if((str2 -str1 -2)<=0)
+    {
+//        printf("can't find address,please use -g compile.");
+//        strncpy(strtem,"can't find address,please use -g compile.",256);
+        str1 = strchr(addr,'[');
+        str2 = strchr(addr, ']');
+        if(str1 == NULL || str2 == NULL)
+        {
+            return;
+        }
+        if((str2 -str1 -2)<=0)
+        {
+            printf("can't find address,please use -g compile.");
+            strncpy(strtem,"can't find address,please use -g compile.",256);
+            return;
+        }
+    }
+
+    memcpy(addrline, str1 + 1, str2 -str1 -1);
+    snprintf(cmd, sizeof(cmd), "addr2line -e /proc/%d/exe %s ", getpid(), addrline);
+    qDebug("%s\n",cmd);
+
+
+    file = popen(cmd, "r");
+    if(NULL != fgets(line, 256, file))
+    {
+        printf("%s\n", line);
+        snprintf(strtem,1000,"%s\n",line);
+    }
+    if(strnlen(line,255) == 0)
+    {
+        printf("no addr.\n");
+    }
+    pclose(file);
+}
+
+static void proc_addrline(int64_t xaddr,char * strtem)
+{
+    char cmd[256];
+    char line[256];
+    FILE* file;
+
+    snprintf(strtem,1000," ");
+    snprintf(cmd, sizeof(cmd), "addr2line -e /proc/%d/exe %x ", getpid(), xaddr);
+    qDebug("%s\n",cmd);
+
+    file = popen(cmd, "r");
+    if(NULL != fgets(line, 256, file))
+    {
+        printf("%s\n", line);
+        snprintf(strtem,1000,"%s\n",line);
+    }
+    if(strnlen(line,255) == 0)
+    {
+        printf("no addr.\n");
+    }
+    pclose(file);
+
+}
+
+void out_stack(char *sig)
+{
+    void *array[32];
+    size_t size;
+    char **strings;
+    size_t i;
+
+    char stroutput[6000];
+    char strtem[1000];
+
+    snprintf(stroutput,3000," ");
+    printf("%s\n", sig);
+    qDebug("%s\n", sig);
+    size = backtrace (array, 32);
+    //该函数用于获取当前线程的调用堆栈,获取的信息将会被存放在array中,
+    //它是一个指针列表。参数 32 用来指定array中可以保存多少个void* 元素。
+    //函数返回值是实际获取的指针个数,最大不超过32大小.
+    strings = backtrace_symbols (array, size);
+
+    /* backtrace_symbols将从backtrace函数获取的信息转化为一个字符串数组.
+     * 参数array应该是从backtrace函数获取的指针数组,
+     * size是该数组中的元素个数(backtrace的返回值)
+     * 函数返回值是一个指向字符串数组的指针,它的大小同array相同.
+     * 每个字符串包含了一个相对于array中对应元素的可打印信息.
+     * 它包括函数名，函数的偏移地址,和实际的返回地址
+     */
+
+
+
+    if (NULL == strings)
+    {
+        printf("backtrace_symbols\n");
+        return ;
+    }
+
+
+    qDebug("size is %d",(int)size);
+
+    for (i = 0; i <size; i++)
+    {
+//        qDebug("i is %d",i);
+        printf("%s\n",strings[i]);
+//        snprintf(strtem,1000,"%s\n",strings[i]);
+//        strncat(stroutput,strtem,6000);
+//        output_addrline(strings[i],strtem);
+//        strncat(stroutput,strtem,6000);
+    }
+
+    for(i=0;i<size;i++)
+    {
+        int64_t xaddr = (int64_t)((int *)array[i]);
+        printf("addr is %x\n", xaddr);
+        snprintf(strtem,1000,"addr is %x\n", xaddr);
+        strncat(stroutput,strtem,6000);
+        proc_addrline(xaddr,strtem);
+        strncat(stroutput,strtem,6000);
+    }
+
+
+    free(strings);
+
+    savetofile(stroutput);
+
+    QTime x;
+    x.start();
+    while((unsigned int)(x.elapsed())<100)
+    {
+ //       qDebug("hello");
+    }
+
+
+
+}
 
 
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
+    signal(SIGABRT, signal_exit);
+    signal(SIGTERM, signal_exit);
+    signal(SIGSEGV, signal_exit);
+
+//    int * p = 0;
+
+//    *p = 1;
+
+    std::cout<<QDateTime::currentDateTime().toString("yyyy-MM-dd-hh-mm-ss-zzz:").toStdString()<<std::endl;
+
     snprintf(gstrport,255,"50061");
 
     int nRtn = GetOptLong(argc,argv);

src/driver/driver_cloud_swap_server/swapunit.cpp

@@ -1,5 +1,7 @@
 #include "swapunit.h"
 
+#include <QDateTime>
+
 swapunit::swapunit(std::string strnodeid,std::string strobjnodeid,::grpc::ServerReaderWriter<iv::CloudSwapReplyStream, iv::CloudSwapRequestStream>* stream)
 {
     mstrnodeid = strnodeid;
@@ -33,7 +35,7 @@ int swapunit::sendmsg(std::string strobjid, std::shared_ptr<char> pdata_ptr, int
 
     if(mvectorsendmsgbuf.size()>30000)
     {
-        std::cout<<"may connection down, buffer now full."<<std::endl;
+        std::cout<<QDateTime::currentDateTime().toString("yyyy-MM-dd-hh-mm-ss-zzz:").toStdString()<<"may connection down, buffer now full."<<std::endl;
         mmutex.lock();
         mvectorsendmsgbuf.clear();
         mmutex.unlock();
@@ -99,9 +101,10 @@ void swapunit::threadsend(::grpc::ServerReaderWriter<iv::CloudSwapReplyStream, i
             flatencyinnodestore = xvectorsendmsgbuf[0].flatency_nodestore;
 
         }
+
         for(i=0;i<nsize;i++)
         {
-            std::cout<<" add reply data. "<<std::endl;
+            std::cout<<QDateTime::currentDateTime().toString("yyyy-MM-dd-hh-mm-ss-zzz:").toStdString()<<" add reply data. "<<std::endl;
             xreply.add_xdata(xvectorsendmsgbuf[i].pdata_ptr.get(),xvectorsendmsgbuf[i].ndatasize);
         }
         xreply.set_flatencyinstore_obj(flatencyinnodestore);
@@ -109,7 +112,7 @@ void swapunit::threadsend(::grpc::ServerReaderWriter<iv::CloudSwapReplyStream, i
         xreply.set_flatency_inobjnode(fpinginobj);
         xreply.set_nmsgservertime(xNow);
  //       xreply.set_nres(1);
-        std::cout<<mstrnodeid<<" reply data size : "<<xreply.xdata_size()<<std::endl;
+        std::cout<<QDateTime::currentDateTime().toString("yyyy-MM-dd-hh-mm-ss-zzz:").toStdString()<<mstrnodeid<<" reply data size : "<<xreply.xdata_size()<<std::endl;
         stream->Write(xreply);
 
         xLastSend = xNow;
@@ -126,6 +129,7 @@ bool swapunit::CanDel()
 void swapunit::stopswap()
 {
     mbrun = false;
+    mpthread->join();
 }
 
 void swapunit::SetPingAvg(double fpingavg)

src/driver/driver_lidar_leishen32/leishen32.cpp

@@ -133,7 +133,7 @@ int leishen32::DecodeUDPPac(iv::lidarudppac & xpac)
         fazu = fazu * 0.01;
 
         pcl::PointXYZI point;
- //       std::cout<<" fazu: "<<fazu<<std::endl;
+        std::cout<<" fazu: "<<fazu<<std::endl;
 
 
         if(fabs(fazu-gAngleOld)>=300)

src/driver/driver_lidar_leishen32/main.cpp

@@ -181,7 +181,7 @@ void decodeyaml(const char * stryaml)
 
     if(config["devport"])
     {
-        strncpy(gstr_devport,config["port"].as<std::string>().data(),255);
+        strncpy(gstr_devport,config["devport"].as<std::string>().data(),255);
     }
 
     if(config["devmode"])

src/driver/driver_lidar_leishen_c16/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/driver/driver_lidar_leishen_c16/driver_lidar_leishen_c16.pro

@@ -0,0 +1,42 @@
+QT -= gui
+
+
+QT       += network
+
+CONFIG += c++14 #console
+CONFIG -= app_bundle
+
+# 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 \
+    leishenc16.cpp \
+    ../common/lidar/lidardecode.cpp \
+    ../common/lidar/lidarudp.cpp
+
+HEADERS += \
+    leishenc16.h \
+    ../common/lidar/lidardecode.h \
+    ../common/lidar/lidarudp.h
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+
+!include(../../../include/ivyaml-cpp.pri ) {
+    error( "Couldn't find the ivyaml-cpp.pri file!" )
+}
+
+INCLUDEPATH += $$PWD/../common/lidar

src/driver/driver_lidar_leishen_c16/leishenc16.cpp

@@ -0,0 +1,324 @@
+#include "leishenc16.h"
+
+#include "ivstdcolorout.h"
+
+
+leishenc16::leishenc16(char * strmemname,double froll,double finclinationang_xaxis ,
+                     double finclinationang_yaxis , int nDevMode ,
+                     unsigned short nDataPort,unsigned short nDevPort )
+{
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
+    point_cloud->header.frame_id = "velodyne";
+    point_cloud->height = 1;
+    point_cloud->header.stamp = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
+    point_cloud->width = 0;
+    point_cloud->header.seq =0;
+    mpoint_cloud_temp = point_cloud;
+
+
+    RegisterSend(strmemname);
+    mnLidarMode = nDevMode;
+    mfrollang = froll;
+    mfinclinationang_xaxis = finclinationang_xaxis;
+    mfinclinationang_yaxis = finclinationang_yaxis;
+
+        mpleishenc16_vertical = &leishenc16_vertical[0];
+    int i;
+    for(i=0;i<16;i++)
+    {
+        mthetacos[i] = cos(mpleishenc16_vertical[i]*M_PI/180.0);
+        mthetasin[i] = sin(mpleishenc16_vertical[i]*M_PI/180.0);
+        std::cout<<"sin "<<i<<" "<<mthetasin[i]<<std::endl;
+        std::cout<<"cos "<<i<<" "<<mthetacos[i]<<std::endl;
+    }
+
+    mplidarudp = new lidarudp(nDataPort);
+    mplidardevudp = new lidarudp(nDevPort);
+    mpthreaddev = new std::thread(&leishenc16::threaddevdecode,this);
+    mpthread = new std::thread(&leishenc16::threaddecode,this);
+}
+
+leishenc16::~leishenc16()
+{
+    mbrun = false;
+    mpthreaddev->join();
+    mpthread->join();
+    delete mplidarudp;
+    delete mplidardevudp;
+}
+
+void leishenc16::threaddevdecode()
+{
+    std::vector<iv::lidarudppac> xvectorlidarudppac;
+    while(mbrun)
+    {
+        xvectorlidarudppac = mplidardevudp->ComsumeRecv(100);
+        if(xvectorlidarudppac.size()>0)
+        {
+            unsigned int i;
+            for(i=0;i<xvectorlidarudppac.size();i++)
+            {
+                DecodeDevinfo(xvectorlidarudppac[i]);
+            }
+        }
+    }
+}
+
+void leishenc16::threaddecode()
+{
+    std::vector<iv::lidarudppac> xvectorlidarudppac;
+    while(mbrun)
+    {
+        xvectorlidarudppac = mplidarudp->ComsumeRecv(10);
+        if(xvectorlidarudppac.size()>0)
+        {
+            unsigned int i;
+            for(i=0;i<xvectorlidarudppac.size();i++)
+            {
+                DecodeUDPPac(xvectorlidarudppac[i]);
+            }
+        }
+    }
+}
+
+int leishenc16::DecodeUDPPac(iv::lidarudppac & xpac)
+{
+    if(xpac.mndatasize != 1212)
+    {
+        return  -1;
+    }
+    static int nNotDecodeNum = 0;
+    if(mbdevinfo == false)
+    {
+        nNotDecodeNum++;
+        if(nNotDecodeNum > 1000)
+        {
+            ivstdcolorout("No Device Info Packet,Can't Get A1 A2 A3 A4.",iv::STDCOLOR_BOLDYELLOW);
+        }
+        return 0;
+    }
+    nNotDecodeNum = 0;
+
+    double finclinationang_xaxis = mfinclinationang_xaxis * M_PI/180.0;// atof(gstr_inclinationang_xaxis)*M_PI/180.0;  //Inclination from x axis
+    double finclinationang_yaxis = mfinclinationang_yaxis * M_PI/180.0;// atof(gstr_inclinationang_yaxis)*M_PI/180.0;   //Inclination from y axis
+    bool binclix = false;
+    bool bincliy = false;
+    if(fabs(finclinationang_xaxis)>0.00000001)binclix = true;
+    if(fabs(finclinationang_yaxis)>0.00000001)bincliy = true;
+
+    float cos_finclinationang_xaxis = static_cast<float>(cos(finclinationang_xaxis)) ;
+    float sin_finclinationang_xaxis = static_cast<float>(sin(finclinationang_xaxis));
+    float cos_finclinationang_yaxis = static_cast<float>(cos(finclinationang_yaxis));
+    float sin_finclinationang_yaxis = static_cast<float>(sin(finclinationang_yaxis));
+
+    double frollang = mfrollang *M_PI/180.0;
+
+    char * pstr =  static_cast<char * >(xpac.mdata_ptr.get()) ;
+
+    static double gAngleTotal = 0;
+    static double gAngleOld = 0;
+    static unsigned int g_seq = 1;
+
+
+
+    int i;
+    for(i=0;i<12;i++)
+    {
+        char * pstrpac = pstr+i*100;
+        unsigned short * pazu = reinterpret_cast<unsigned short * >(pstrpac + 2);
+        double fazu = *pazu;
+        fazu = fazu * 0.01;
+
+        pcl::PointXYZI point;
+ //       std::cout<<" fazu: "<<fazu<<std::endl;
+
+
+        if(fabs(fazu-gAngleOld)>=300)
+        {
+            gAngleTotal = gAngleTotal + fabs(fabs(fazu-gAngleOld)-360);
+ //           std::cout<<" add : "<<fabs(fabs(fazu-gAngleOld)-360)<<std::endl;
+        }
+        else
+        {
+            gAngleTotal = gAngleTotal + fabs(fabs(fazu-gAngleOld));
+        }
+
+        gAngleOld = fazu;
+
+        fazu = fazu * M_PI/180.0;
+
+
+        if(gAngleTotal>360.0)
+        {
+            gAngleTotal = 0.0;
+            std::cout<< std::chrono::system_clock::now().time_since_epoch().count()/1000000 <<" share point cloud."<<std::endl;
+            ProduceCloud(mpoint_cloud_temp);
+            std::cout<<" produce. "<<std::endl;
+            //share point cloud.
+
+            pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                        new pcl::PointCloud<pcl::PointXYZI>());
+
+            point_cloud->header.frame_id = "velodyne";
+            point_cloud->height = 1;
+            point_cloud->header.stamp = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
+            point_cloud->width = 0;
+            point_cloud->header.seq =g_seq;
+            g_seq++;
+            mpoint_cloud_temp = point_cloud;
+        }
+
+
+        int j;
+        for(j=0;j<16;j++)
+        {
+            double Range = ((pstrpac[4+j*3+1] << 8) + pstrpac[4+j*3+0]);
+            unsigned char intensity = static_cast<unsigned char>(pstr[4+j*3+2]) ;
+            Range=Range* 0.004;
+
+
+            pcl::PointXYZI point;
+  //          std::cout<<" fazu: "<<fazu<<" cos theta : "<<mthetacos[j] <<" range."<<Range<< std::endl;
+
+            point.x  = static_cast<float>(Range* mthetacos[j]*cos(frollang -fazu   )) ;
+            point.y  = static_cast<float>(Range* mthetacos[j] *sin(frollang -fazu ));
+            point.z  = static_cast<float>(Range* mthetasin[j]);
+            point.intensity = intensity;
+
+            if(binclix)
+            {
+                float y,z;
+                y = point.y;z = point.z;
+                point.y = y*cos_finclinationang_xaxis +z*sin_finclinationang_xaxis;
+                point.z = z*cos_finclinationang_xaxis - y*sin_finclinationang_xaxis;
+            }
+            if(bincliy)
+            {
+                float z,x;
+                z = point.z;x = point.x;
+                point.z = z*cos_finclinationang_yaxis + x*sin_finclinationang_yaxis;
+                point.x = x*cos_finclinationang_yaxis - z*sin_finclinationang_yaxis;
+            }
+  //          std::cout<<" x: "<<point.x<<" y: "<<point.y<<std::endl;
+
+            mpoint_cloud_temp->points.push_back(point);
+
+
+            ++mpoint_cloud_temp->width;
+        }
+
+        fazu = fazu + 0.18 * M_PI/180.0;
+
+        for(j=0;j<16;j++)
+        {
+            double Range = ((pstrpac[4+j*3+1 + 48] << 8) + pstrpac[4+j*3+0 + 48]);
+            unsigned char intensity = static_cast<unsigned char>(pstr[4+j*3+2 + 48]) ;
+            Range=Range* 0.004;
+
+
+            pcl::PointXYZI point;
+ //           std::cout<<" fazu: "<<fazu<<" cos theta : "<<mthetacos[j] <<" range."<<Range<< std::endl;
+
+            point.x  = static_cast<float>(Range* mthetacos[j]*cos(frollang -fazu  )) ;
+            point.y  = static_cast<float>(Range* mthetacos[j] *sin(frollang -fazu  ));
+            point.z  = static_cast<float>(Range* mthetasin[j]);
+            point.intensity = intensity;
+
+            if(binclix)
+            {
+                float y,z;
+                y = point.y;z = point.z;
+                point.y = y*cos_finclinationang_xaxis +z*sin_finclinationang_xaxis;
+                point.z = z*cos_finclinationang_xaxis - y*sin_finclinationang_xaxis;
+            }
+            if(bincliy)
+            {
+                float z,x;
+                z = point.z;x = point.x;
+                point.z = z*cos_finclinationang_yaxis + x*sin_finclinationang_yaxis;
+                point.x = x*cos_finclinationang_yaxis - z*sin_finclinationang_yaxis;
+            }
+  //          std::cout<<" x: "<<point.x<<" y: "<<point.y<<std::endl;
+
+            mpoint_cloud_temp->points.push_back(point);
+
+
+            ++mpoint_cloud_temp->width;
+        }
+
+    }
+
+    return  0;
+
+}
+
+int leishenc16::DecodeDevinfo(iv::lidarudppac & xpac)
+{
+    if(xpac.mndatasize != 1206)
+    {
+        return  -1;
+    }
+
+    char * pstr =  static_cast<char * >(xpac.mdata_ptr.get()) ;
+ //   unsigned short  * pmoterspeed = reinterpret_cast<unsigned short *>(pstr+8);//  ( unsigned short *)(pstr+8);
+  //  qDebug("%02x %02x",pstr[8],pstr[9]);
+    mdevinfo.motorspeed = pstr[8] * 256 + pstr[9];
+ //   mdevinfo.motorspeed = *pmoterspeed;
+
+    std::cout<<" motor speed: "<<mdevinfo.motorspeed<<std::endl;
+
+    unsigned short * pmotorun = reinterpret_cast<unsigned short * >(pstr+40);
+    if(*pmotorun == 1)
+    {
+        mdevinfo.bmotorrun = true;
+    }
+    else
+        mdevinfo.bmotorrun = false;
+
+    unsigned short * ppacinterval = reinterpret_cast<unsigned short * >(pstr+42);
+    mdevinfo.pacinterval = *ppacinterval;
+
+    double A[4];
+    int i;
+    for(i=0;i<4;i++)
+    {
+        unsigned short * pA = reinterpret_cast<unsigned short *>(pstr+186+i*2);
+        unsigned short Avalue = *pA;
+        A[i] =((pstr[186+i*2+0] << 8) + pstr[186+i*2+1]);   //Avalue;
+        A[i] = A[i]*0.01;
+    }
+
+//    mdevinfo.A1 = A[0];
+//    mdevinfo.A2 = A[2];
+//    mdevinfo.A3 = A[1];
+//    mdevinfo.A4 = A[3];
+
+//    if(mnLidarMode == 0)
+//    {
+//        mpleishenc16_A[0] = mdevinfo.A2;mpleishenc16_A[1] = mdevinfo.A2;mpleishenc16_A[2] = mdevinfo.A1;mpleishenc16_A[3] = mdevinfo.A1;
+//        mpleishenc16_A[4] = mdevinfo.A2;mpleishenc16_A[5] = mdevinfo.A2;mpleishenc16_A[6] = mdevinfo.A1;mpleishenc16_A[7] = mdevinfo.A1;
+//        mpleishenc16_A[8] = mdevinfo.A2;mpleishenc16_A[9] = mdevinfo.A2;mpleishenc16_A[10] = mdevinfo.A1;mpleishenc16_A[11] = mdevinfo.A1;
+//        mpleishenc16_A[12] = mdevinfo.A2;mpleishenc16_A[13] = mdevinfo.A2;mpleishenc16_A[14] = mdevinfo.A1;mpleishenc16_A[15] = mdevinfo.A1;
+//        mpleishenc16_A[16] = mdevinfo.A2;mpleishenc16_A[17] = mdevinfo.A2;mpleishenc16_A[18] = mdevinfo.A1;mpleishenc16_A[19] = mdevinfo.A1;
+//        mpleishenc16_A[20] = mdevinfo.A2;mpleishenc16_A[21] = mdevinfo.A2;mpleishenc16_A[22] = mdevinfo.A1;mpleishenc16_A[23] = mdevinfo.A1;
+//        mpleishenc16_A[24] = mdevinfo.A2;mpleishenc16_A[25] = mdevinfo.A2;mpleishenc16_A[26] = mdevinfo.A1;mpleishenc16_A[27] = mdevinfo.A1;
+//        mpleishenc16_A[28] = mdevinfo.A2;mpleishenc16_A[29] = mdevinfo.A2;mpleishenc16_A[30] = mdevinfo.A1;mpleishenc16_A[31] = mdevinfo.A1;
+
+//    }
+//    else
+//    {
+//        mpleishenc16_A[0] = mdevinfo.A2;mpleishenc16_A[1] = mdevinfo.A2;mpleishenc16_A[2] = mdevinfo.A1;mpleishenc16_A[3] = mdevinfo.A1;
+//        mpleishenc16_A[4] = mdevinfo.A2;mpleishenc16_A[5] = mdevinfo.A4;mpleishenc16_A[6] = mdevinfo.A1;mpleishenc16_A[7] = mdevinfo.A3;
+//        mpleishenc16_A[8] = mdevinfo.A2;mpleishenc16_A[9] = mdevinfo.A2;mpleishenc16_A[10] = mdevinfo.A1;mpleishenc16_A[11] = mdevinfo.A1;
+//        mpleishenc16_A[12] = mdevinfo.A2;mpleishenc16_A[13] = mdevinfo.A4;mpleishenc16_A[14] = mdevinfo.A1;mpleishenc16_A[15] = mdevinfo.A3;
+//        mpleishenc16_A[16] = mdevinfo.A2;mpleishenc16_A[17] = mdevinfo.A2;mpleishenc16_A[18] = mdevinfo.A1;mpleishenc16_A[19] = mdevinfo.A1;
+//        mpleishenc16_A[20] = mdevinfo.A4;mpleishenc16_A[21] = mdevinfo.A2;mpleishenc16_A[22] = mdevinfo.A3;mpleishenc16_A[23] = mdevinfo.A1;
+//        mpleishenc16_A[24] = mdevinfo.A2;mpleishenc16_A[25] = mdevinfo.A2;mpleishenc16_A[26] = mdevinfo.A1;mpleishenc16_A[27] = mdevinfo.A1;
+//        mpleishenc16_A[28] = mdevinfo.A4;mpleishenc16_A[29] = mdevinfo.A2;mpleishenc16_A[30] = mdevinfo.A3;mpleishenc16_A[31] = mdevinfo.A1;
+//    }
+
+    mbdevinfo = true;
+    return 0;
+}

src/driver/driver_lidar_leishen_c16/leishenc16.h

@@ -0,0 +1,81 @@
+#ifndef leishenc16_H
+#define leishenc16_H
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <vector>
+#include <mutex>
+#include <condition_variable>
+#include <thread>
+
+#include "lidarudp.h"
+
+#include "lidardecode.h"
+
+
+
+namespace iv {
+struct leishenc16_devinfo
+{
+    int motorspeed;
+    bool bmotorrun;
+    int pacinterval;
+    double A1;
+    double A2;
+    double A3;
+    double A4;
+
+};
+}
+
+class leishenc16 : public lidardecode
+{
+public:
+    leishenc16(char * strmemname, double froll = 0.0,double finclinationang_xaxis = 0.0,
+              double finclinationang_yaxis = 0.0, int nDevMode = 0,
+              unsigned short nDataPort = 2368,unsigned short nDevPort = 2369);
+    ~leishenc16();
+
+private:
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud_temp;
+
+    iv::leishenc16_devinfo mdevinfo;
+    bool mbdevinfo = false;
+
+    const double * mpleishenc16_vertical;
+    double  mpleishenc16_A[16];
+
+    double mthetacos[16];
+    double mthetasin[16];
+
+    std::thread * mpthread;
+    std::thread * mpthreaddev;
+    lidarudp * mplidarudp;
+    lidarudp * mplidardevudp;
+
+    bool mbrun = true;
+
+    int mnLidarMode = 0; //0 1degree   1 0.33degree
+
+    double mfinclinationang_xaxis = 0.0;
+    double mfinclinationang_yaxis = 0.0;
+    double mfrollang = 0.0;
+
+private:
+    const double leishenc16_vertical[16]={-16,0,-14,2,-12,4,-10,6,
+                                    -8,8,-6,10,-4,12,-2,14};
+
+
+
+private:
+    void threaddecode( );
+    void threaddevdecode();
+    int DecodeUDPPac(iv::lidarudppac & xpac);
+    int DecodeDevinfo(iv::lidarudppac & xpac);
+
+
+};
+
+#endif // leishenc16_H

src/driver/driver_lidar_leishen_c16/main.cpp

@@ -0,0 +1,234 @@
+#include <QCoreApplication>
+
+
+#include "leishenc16.h"
+
+#include <getopt.h>
+#include <yaml-cpp/yaml.h>   //if no this,sudo apt-get install libyaml-cpp-dev
+
+#include "ivfault.h"
+#include "ivlog.h"
+
+
+static char gstr_memname[256];
+static char gstr_rollang[256];
+static char gstr_inclinationang_yaxis[256];  //from y axis
+static char gstr_inclinationang_xaxis[256];  //from x axis
+//char gstr_hostip[256];
+static char gstr_port[256];
+static char gstr_devport[256];
+static char gstr_yaml[256];
+static char gstr_modulename[256];
+static char gstr_devmode[256];
+
+/**
+ * @brief print_useage
+ */
+void print_useage()
+{
+    std::cout<<" -n --modulename $modulename : set module name. eq.  -n lidarleft"<<std::endl;
+    std::cout<<" -m --memname $memname : share memory name. eq.  -m lidar_pc"<<std::endl;
+    std::cout<<" -r --rollang $rollang : roll angle. eq.  -r 10.0"<<std::endl;
+    std::cout<<" -x --inclinationang_xaxis $inclinationang_xaxis : inclination angle from x axis. eq.  -x 0.0"<<std::endl;
+    std::cout<<" -y --inclinationang_yaxis $inclinationang_yaxis : inclination angle from y axis. eq.  -y 0.0"<<std::endl;
+//    std::cout<<" -o --hostip $hostip : host ip. eq.  -o 192.168.1.111"<<std::endl;
+    std::cout<<" -p --port $port : port . eq.  -p 2368"<<std::endl;
+    std::cout<<" -d --devport $port : port . eq.  -d 2369"<<std::endl;
+    std::cout<<" -f --devport $port : port . eq.  -f 0"<<std::endl;
+    std::cout<<" -s --setyaml $yaml : port . eq.  -s rs1.yaml"<<std::endl;
+    std::cout<<" -h --help print help"<<std::endl;
+}
+
+int  GetOptLong(int argc, char *argv[]) {
+    int nRtn = 0;
+    int opt; // getopt_long() 的返回值
+    int digit_optind = 0; // 设置短参数类型及是否需要参数
+    (void)digit_optind;
+
+    // 如果option_index非空，它指向的变量将记录当前找到参数符合long_opts里的
+    // 第几个元素的描述，即是long_opts的下标值
+    int option_index = 0;
+    // 设置短参数类型及是否需要参数
+    const char *optstring = "n:m:r:x:y:p:d:f:s:h";
+
+    // 设置长参数类型及其简写，比如 --reqarg <==>-r
+    /*
+    struct option {
+             const char * name;  // 参数的名称
+             int has_arg; // 是否带参数值，有三种：no_argument， required_argument，optional_argument
+             int * flag; // 为空时，函数直接将 val 的数值从getopt_long的返回值返回出去，
+                     // 当非空时，val的值会被赋到 flag 指向的整型数中，而函数返回值为0
+             int val; // 用于指定函数找到该选项时的返回值，或者当flag非空时指定flag指向的数据的值
+        };
+    其中：
+        no_argument(即0)，表明这个长参数不带参数（即不带数值，如：--name）
+            required_argument(即1)，表明这个长参数必须带参数（即必须带数值，如：--name Bob）
+            optional_argument(即2)，表明这个长参数后面带的参数是可选的，（即--name和--name Bob均可）
+     */
+    static struct option long_options[] = {
+        {"modulename", required_argument, NULL, 'n'},
+        {"memname", required_argument, NULL, 'm'},
+        {"rollang", required_argument, NULL, 'r'},
+        {"inclinationang_xaxis", required_argument, NULL, 'x'},
+        {"inclinationang_yaxis", required_argument, NULL, 'y'},
+        {"port", required_argument, NULL, 'p'},
+        {"devport", required_argument, NULL, 'd'},
+        {"devmode", required_argument, NULL, 'f'},
+        {"setyaml", required_argument, NULL, 's'},
+        {"help",  no_argument,       NULL, 'h'},
+ //       {"optarg", optional_argument, NULL, 'o'},
+        {0, 0, 0, 0}  // 添加 {0, 0, 0, 0} 是为了防止输入空值
+    };
+
+    while ( (opt = getopt_long(argc,
+                               argv,
+                               optstring,
+                               long_options,
+                               &option_index)) != -1) {
+//        printf("opt = %c\n", opt); // 命令参数，亦即 -a -b -n -r
+//        printf("optarg = %s\n", optarg); // 参数内容
+//        printf("optind = %d\n", optind); // 下一个被处理的下标值
+//        printf("argv[optind - 1] = %s\n",  argv[optind - 1]); // 参数内容
+//        printf("option_index = %d\n", option_index);  // 当前打印参数的下标值
+//        printf("\n");
+        switch(opt)
+        {
+        case 'n':
+            strncpy(gstr_modulename,optarg,255);
+            break;
+        case 'm':
+            strncpy(gstr_memname,optarg,255);
+            break;
+        case 'r':
+            strncpy(gstr_rollang,optarg,255);
+            break;
+        case 'x':
+            strncpy(gstr_inclinationang_xaxis,optarg,255);
+            break;
+        case 'y':
+            strncpy(gstr_inclinationang_yaxis,optarg,255);
+            break;
+//        case 'o':
+//            strncpy(gstr_hostip,optarg,255);
+//            break;
+        case 'p':
+            strncpy(gstr_port,optarg,255);
+            break;
+        case 'd':
+            strncpy(gstr_devport,optarg,255);
+            break;
+        case 'f':
+            strncpy(gstr_devmode,optarg,255);
+            break;
+        case 's':
+            strncpy(gstr_yaml,optarg,255);
+            break;
+        case 'h':
+            print_useage();
+            nRtn = 1; //because use -h
+            break;
+        default:
+            break;
+        }
+
+    }
+
+    return nRtn;
+}
+
+/**
+ * @brief decodeyaml
+ * @param stryaml yaml path
+ */
+void decodeyaml(const char * stryaml)
+{
+    YAML::Node config;
+    try
+    {
+        config = YAML::LoadFile(stryaml);
+    }
+    catch(YAML::BadFile e)
+    {
+        qDebug("load yaml error.");
+        return;
+    }
+
+
+    if(config["memname"])
+    {
+        strncpy(gstr_memname,config["memname"].as<std::string>().data(),255);
+    }
+    if(config["rollang"])
+    {
+        strncpy(gstr_rollang,config["rollang"].as<std::string>().data(),255);
+    }
+    if(config["inclinationang_xaxis"])
+    {
+        strncpy(gstr_inclinationang_xaxis,config["inclinationang_xaxis"].as<std::string>().data(),255);
+    }
+    if(config["inclinationang_yaxis"])
+    {
+        strncpy(gstr_inclinationang_yaxis,config["inclinationang_yaxis"].as<std::string>().data(),255);
+    }
+//    if(config["hostip"])
+//    {
+//        strncpy(gstr_hostip,config["hostip"].as<std::string>().data(),255);
+//    }
+    if(config["port"])
+    {
+        strncpy(gstr_port,config["port"].as<std::string>().data(),255);
+    }
+
+    if(config["devport"])
+    {
+        strncpy(gstr_devport,config["devport"].as<std::string>().data(),255);
+    }
+
+    if(config["devmode"])
+    {
+        strncpy(gstr_devmode,config["port"].as<std::string>().data(),255);
+    }
+
+//    std::cout<<gstr_memname<<std::endl;
+//    std::cout<<gstr_rollang<<std::endl;
+//    std::cout<<gstr_inclinationang_xaxis<<std::endl;
+//    std::cout<<gstr_inclinationang_yaxis<<std::endl;
+//    std::cout<<gstr_hostip<<std::endl;
+//    std::cout<<gstr_port<<std::endl;
+}
+
+
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    snprintf(gstr_memname,255,"lidar_pc");
+    snprintf(gstr_rollang,255,"270.0");//这个角度要根据安装情况进行校正！！！！！！！！
+    snprintf(gstr_inclinationang_xaxis,255,"0.0");
+    snprintf(gstr_inclinationang_yaxis,255,"0");
+//    snprintf(gstr_hostip,255,"192.168.1.102");
+    snprintf(gstr_port,255,"2368");//默认端口号
+    snprintf(gstr_devport,255,"2369");//默认端口号
+    snprintf(gstr_devmode,255,"0");//默认端口号
+    snprintf(gstr_yaml,255," ");
+    snprintf(gstr_modulename,255,"driver_lidar_leishenc16");
+
+    int nRtn = GetOptLong(argc,argv);
+    if(nRtn == 1)  //show help,so exit.
+    {
+        return 0;
+    }
+
+    if(strnlen(gstr_yaml,255)>1)
+    {
+        decodeyaml(gstr_yaml);
+    }
+
+    leishenc16 * pleishenc16 = new leishenc16(gstr_memname,atof(gstr_rollang),atof(gstr_inclinationang_xaxis),
+                                           atof(gstr_inclinationang_yaxis),atoi(gstr_devmode),static_cast<unsigned short>(atoi(gstr_port)),
+                                           static_cast<unsigned short>(atoi(gstr_devport)));
+    (void)pleishenc16;
+
+    return a.exec();
+}

src/driver/driver_lidar_rs16/driver_lidar_rs16.pro

@@ -2,7 +2,7 @@ QT -= gui
 
 QT += network
 
-CONFIG += c++11 console
+CONFIG += c++14 console
 CONFIG -= app_bundle
 
 # The following define makes your compiler emit warnings if you use

src/driver/driver_lidar_rs16p/driver_lidar_rs16p.pro

@@ -2,7 +2,7 @@ QT -= gui
 
 QT += network
 
-CONFIG += c++14 console
+CONFIG += c++14 # console
 CONFIG -= app_bundle
 
 # The following define makes your compiler emit warnings if you use

src/driver/driver_lidar_rs16p/main.cpp

@@ -1,8 +1,12 @@
 #include <QCoreApplication>
 
+#include "rs16p.h"
+
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
+    rs16p * prs16p = new rs16p("lidar_pc");
+
     return a.exec();
 }

src/driver/driver_lidar_rs16p/rs16p.cpp

@@ -27,10 +27,18 @@ rs16p::rs16p(char * strmemname, double froll ,double finclinationang_xaxis ,
         mthetacos[i] = cos(mprs16p_vertical[i]*M_PI/180.0);
         mthetasin[i] = sin(mprs16p_vertical[i]*M_PI/180.0);
         std::cout<<"sin "<<i<<" "<<mthetasin[i]<<std::endl;
+
+        mfHor[i] = 0;
     }
 
     mplidarudp = new lidarudp(nDataPort);
+  //   mplidarudp = new lidarudp(7788);
+ //   lidarudp * pdev = new lidarudp(7788);
     mpthread = new std::thread(&rs16p::threaddecode,this);
+
+    int nDevPort = 7788;
+    mplidardevudp = new lidarudp(nDevPort);
+    mpthreaddev = new std::thread(&rs16p::threaddevdecode,this);
 }
 
 rs16p::~rs16p()
@@ -59,12 +67,14 @@ void rs16p::threaddecode( )
 
 int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
 {
-    if(xpac.mndatasize != 1206)
+    if(xpac.mndatasize != 1248)
     {
         std::cout<<" DecodeUDPPac: "<<" pac size is not 1206.  pac size: "<<xpac.mndatasize<<std::endl;
         return  -1;
     }
 
+
+
     double finclinationang_xaxis = mfinclinationang_xaxis * M_PI/180.0;// atof(gstr_inclinationang_xaxis)*M_PI/180.0;  //Inclination from x axis
     double finclinationang_yaxis = mfinclinationang_yaxis * M_PI/180.0;// atof(gstr_inclinationang_yaxis)*M_PI/180.0;   //Inclination from y axis
     bool binclix = false;
@@ -90,11 +100,13 @@ int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
     int i;
     for(i=0;i<12;i++)
     {
-        char * pstrpac = pstr+i*100;
+        char * pstrpac = pstr+i*100 + 42;
         unsigned short * pazu = reinterpret_cast<unsigned short * >(pstrpac + 2);
-        double fazu = *pazu;
+        double fazu =     ((pstrpac[2] << 8) + pstrpac[3]);//*pazu;
         fazu = fazu * 0.01;
 
+  //      std::cout<<" fazu: "<<fazu<<std::endl;
+
         pcl::PointXYZI point;
  //       std::cout<<" fazu: "<<fazu<<std::endl;
 
@@ -111,7 +123,8 @@ int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
 
         gAngleOld = fazu;
 
-        fazu = fazu * M_PI/180.0;
+
+  //      std::cout<<" fazu: "<<fazu<<std::endl;
 
         if(gAngleTotal>=360.0)
         {
@@ -133,10 +146,11 @@ int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
             mpoint_cloud_temp = point_cloud;
         }
 
+         fazu = fazu * M_PI/180.0;
         int j;
         for(j=0;j<16;j++)
         {
-            double Range = ((pstrpac[4+j*3+1] << 8) + pstrpac[4+j*3+0]);
+            double Range = ((pstrpac[4+j*3+0] << 8) + pstrpac[4+j*3+1]);
             unsigned char intensity = static_cast<unsigned char>(pstr[4+j*3+2]) ;
             Range=Range* 0.0025;
 
@@ -144,8 +158,8 @@ int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
             pcl::PointXYZI point;
  //           std::cout<<" fazu: "<<fazu<<" cos theta : "<<mthetacos[j] <<" range."<<Range<< std::endl;
 
-            point.x  = static_cast<float>(Range* mthetacos[j]*cos(frollang -fazu)) ;
-            point.y  = static_cast<float>(Range* mthetacos[j] *sin(frollang -fazu ));
+            point.x  = static_cast<float>(Range* mthetacos[j]*cos(frollang -fazu- mfHor[j])) ;
+            point.y  = static_cast<float>(Range* mthetacos[j] *sin(frollang -fazu - mfHor[j]));
             point.z  = static_cast<float>(Range* mthetasin[j]);
             point.intensity = intensity;
 
@@ -171,11 +185,11 @@ int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
             ++mpoint_cloud_temp->width;
         }
 
-        fazu = fazu + 0.18 * M_PI/180.0;
+        fazu = fazu +0.2 * M_PI/180.0;
 
         for(j=0;j<16;j++)
         {
-            double Range = ((pstrpac[4+j*3+1 + 48] << 8) + pstrpac[4+j*3+0 + 48]);
+            double Range = ((pstrpac[4+j*3+0 + 48] << 8) + pstrpac[4+j*3+1 + 48]);
             unsigned char intensity = static_cast<unsigned char>(pstr[4+j*3+2 + 48]) ;
             Range=Range* 0.0025;
 
@@ -183,8 +197,8 @@ int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
             pcl::PointXYZI point;
  //           std::cout<<" fazu: "<<fazu<<" cos theta : "<<mthetacos[j] <<" range."<<Range<< std::endl;
 
-            point.x  = static_cast<float>(Range* mthetacos[j]*cos(frollang -fazu )) ;
-            point.y  = static_cast<float>(Range* mthetacos[j] *sin(frollang -fazu));
+            point.x  = static_cast<float>(Range* mthetacos[j]*cos(frollang -fazu - mfHor[j] )) ;
+            point.y  = static_cast<float>(Range* mthetacos[j] *sin(frollang -fazu- mfHor[j]));
             point.z  = static_cast<float>(Range* mthetasin[j]);
             point.intensity = intensity;
 
@@ -215,3 +229,50 @@ int rs16p::DecodeUDPPac(iv::lidarudppac & xpac)
 
     return  0;
 }
+
+
+void rs16p::threaddevdecode()
+{
+    std::vector<iv::lidarudppac> xvectorlidarudppac;
+    while(mbrun)
+    {
+        xvectorlidarudppac = mplidardevudp->ComsumeRecv(100);
+        if(xvectorlidarudppac.size()>0)
+        {
+            unsigned int i;
+            for(i=0;i<xvectorlidarudppac.size();i++)
+            {
+                DecodeDevinfo(xvectorlidarudppac[i]);
+            }
+        }
+    }
+}
+
+int rs16p::DecodeDevinfo(iv::lidarudppac & xpac)
+{
+    if(xpac.mndatasize != 1248)
+    {
+        return  -1;
+    }
+
+
+    unsigned char byte[48];
+    memcpy(byte,xpac.mdata_ptr.get()+564,48);
+
+    double fHor;
+    int i;
+    for(i=0;i<16;i++)
+    {
+        fHor = byte[i*3 + 1] * 256.0 + byte[i*3+2];
+        fHor = fHor * 0.01;
+        if(byte[i*3] == 1)fHor = fHor * (-1.0);
+        std::cout<<" hor: "<<i<<" "<<fHor<<std::endl;
+
+        fHor = fHor * M_PI/180.0;
+        mfHor[i] = fHor;
+    }
+    return 0;
+
+
+}
+

src/driver/driver_lidar_rs16p/rs16p.h

@@ -32,9 +32,14 @@ private:
     double mthetacos[16];
     double mthetasin[16];
 
+    double mfHor[16];
+
     std::thread * mpthread;
     lidarudp * mplidarudp;
 
+    std::thread * mpthreaddev;
+    lidarudp * mplidardevudp;
+
     bool mbrun = true;
 
     double mfinclinationang_xaxis = 0.0;
@@ -48,6 +53,9 @@ private:
 private:
     void threaddecode( );
     int DecodeUDPPac(iv::lidarudppac & xpac);
+
+    void threaddevdecode();
+    int DecodeDevinfo(iv::lidarudppac & xpac);
 };
 
 #endif // RS16P_H

src/driver/driver_map_xodrload/driver_map_xodrload.pro

@@ -87,7 +87,7 @@ if(contains(DEFINES,USE_UTM)){
 LIBS += -lGeographic
 }
 
-
+QMAKE_CXXFLAGS +=  -g
 #DEFINES += USE_TMERS
 if(contains(DEFINES,USE_TMERS)){
 INCLUDEPATH += /home/nvidia/Downloads/proj-8.2.1/src

src/driver/driver_map_xodrload/globalplan.cpp

@@ -1152,6 +1152,7 @@ static std::vector<PlanPoint> getarcpoint(GeometryArc * parc,const double fspace
     int nsize = parc->GetLength()/fspace;
     if(nsize == 0)nsize = 1;
     int i;
+
     for(i=0;i<nsize;i++)
     {
         double x,y;
@@ -1504,10 +1505,11 @@ int indexinroadpoint(std::vector<PlanPoint> xvectorPP,double x,double y)
 {
     int nrtn = 0;
     int i;
-    int dismin = 1000;
+    double dismin = 1000;
     for(i=0;i<xvectorPP.size();i++)
     {
         double dis = sqrt(pow(xvectorPP.at(i).x - x,2) + pow(xvectorPP.at(i).y -y,2));
+
         if(dis <dismin)
         {
             dismin = dis;
@@ -2079,6 +2081,8 @@ double getoff(Road * pRoad,int nlane,const double s)
 
     }
 
+    if(nlane<0)foff = foff*(-1);
+
     if(pRoad->GetLaneOffsetCount()>0)
     {
 
@@ -2097,10 +2101,11 @@ double getoff(Road * pRoad,int nlane,const double s)
         double s_off = s - pLO->GetS();
         double off1 = pLO->Geta() + pLO->Getb()*s_off + pLO->Getc() * s_off * s_off
                 +pLO->Getd() * s_off * s_off * s_off;
-        foff = foff - off1;
+        foff = foff + off1;
+
     }
 
-    if(nlane<0)foff = foff*(-1);
+
     return foff;
 }
 

src/driver/driver_map_xodrload/main.cpp

@@ -1415,6 +1415,22 @@ void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned i
         return;
     }
 
+    static int64_t nLastFusionUpdate = 0;
+
+    int64_t nNow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+    if(strncmp(strmemname,"fusion_gpsimu",256) != 0 )
+    {
+        if(abs(nNow - nLastFusionUpdate)<3000)
+        {
+            return;
+        }
+    }
+    else
+    {
+        nLastFusionUpdate = nNow;
+    }
+
     xodrobj xo;
     xo.fhgdsrc = xgpsimu.heading();
     xo.flatsrc = xgpsimu.lat();
@@ -1605,6 +1621,7 @@ int main(int argc, char *argv[])
     gpasrc =iv::modulecomm::RegisterRecv("xodrsrc",ListenSrc);
     gpagps = iv::modulecomm::RegisterRecv(strgpsmsg.data(),UpdateGPSIMU);
     gpav2x = iv::modulecomm::RegisterRecv(strv2xmsg.data(),ListenV2X);
+    void * pafusion = iv::modulecomm::RegisterRecv("fusion_gpsimu",UpdateGPSIMU);
 
     gpaxodrrawreq = iv::modulecomm::RegisterRecv("xodrrawreq",ListenXODRRawReq);
     gpaxodrrawrep = iv::modulecomm::RegisterSend("xodrrawrep",30000000,1);

src/driver/driver_ntrip_client/driver_ntrip_client.pro

@@ -11,7 +11,7 @@ CONFIG -= app_bundle
 # deprecated API in order to know how to port your code away from it.
 DEFINES += QT_DEPRECATED_WARNINGS
 
-#DEFINES += DEBUG_NTRIP_MSG
+DEFINES += DEBUG_NTRIP_MSG
 
 # You can also make your code fail to compile if it uses deprecated APIs.
 # In order to do so, uncomment the following line.

src/driver/driver_ntrip_client/ntrip_client.cpp

@@ -200,7 +200,7 @@ void ntrip_client::SendGPGGA()
     {
 #ifdef DEBUG_NTRIP_MSG
         char strSend[1000];
-        snprintf(strSend,1000,"$GPGGA,102342.00,3948.6248006,N,11633.1924828,E,1,13,1.5,46.4085,M,-9.452,M,99,AAAA*72\r\n");
+        snprintf(strSend,1000,"$GPGGA,102342.00,2821.872855,N,11748.559952,E,1,13,1.5,46.4085,M,-9.452,M,99,AAAA*72\r\n");
         socket_->write(strSend,strlen(strSend));
         socket_->flush();
 #endif

src/fusion/fusion_gpsndt/fusion_gpsndt.pro

@@ -31,7 +31,8 @@ SOURCES += main.cpp \
     ../../include/msgtype/imu.pb.cc \
     ../../include/msgtype/gpsimu.pb.cc \
     ../../include/msgtype/gps.pb.cc \
-    ../../include/msgtype/fusiongpslidar.pb.cc
+    ../../include/msgtype/fusiongpslidar.pb.cc \
+    ../../include/msgtype/chassis.pb.cc
 
 
 
@@ -41,7 +42,8 @@ HEADERS += \
     ../../include/msgtype/imu.pb.h \
     ../../include/msgtype/gpsimu.pb.h \
     ../../include/msgtype/gps.pb.h \
-    ../../include/msgtype/fusiongpslidar.pb.h
+    ../../include/msgtype/fusiongpslidar.pb.h \
+    ../../include/msgtype/chassis.pb.h
 
 
 

src/fusion/fusion_gpsndt/main.cpp

@@ -15,6 +15,7 @@
 #include "gpsimu.pb.h"
 #include "ndtgpspos.pb.h"
 #include "fusiongpslidar.pb.h"
+#include "chassis.pb.h"
 
 #include "ivbacktrace.h"
 #include "ivfault.h"
@@ -49,7 +50,7 @@ QMutex mMutexgps;
 QMutex mMutexndt;
 
 
-void * gpa,* gpb, * gpc, * gpd;
+void * gpa,* gpb, * gpc, * gpd,* gpe;
 
 std::string gstrgpsmsg;
 std::string gstrndtmsg;
@@ -63,6 +64,33 @@ QUdpSocket * gudpSocketGPSIMU;
 
 
 
+static int64_t glastchassitime = 0;
+static double gfChassisVelSpeed;
+
+
+void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+    iv::chassis xchassis;
+    //    static int ncount = 0;
+    if(!xchassis.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"iv::decition::BrainDecition::UpdateChassis ParseFrom Array Error."<<std::endl;
+        return;
+    }
+
+
+    if(xchassis.has_vel())
+    {
+        gfChassisVelSpeed = xchassis.vel()/3.6;
+        glastchassitime = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+ //       std::cout<<" gf Veh speed : "<<gfChassisVelSpeed<<std::endl;
+    }
+
+}
+
 void ListenRaw(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
     static unsigned int nFixCount = 0;
@@ -179,9 +207,17 @@ static void sharefusiongpsimu(iv::fusion::fusiongpslidar * pfusiongpslidar)
     gpsimu.set_roll(pfusiongpslidar->roll());
     gpsimu.set_pitch(pfusiongpslidar->pitch());
 
+
     gpsimu.set_rtk_state(pfusiongpslidar->state());
     gpsimu.set_ins_state(4);
 
+    int64_t nmsnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    if(abs(nmsnow - glastchassitime) < 1000)
+    {
+        gpsimu.set_speed(gfChassisVelSpeed);
+        std::cout<<"use chassis speed."<<std::endl;
+    }
+
     char * strser;
     bool bser;
     int nbytesize;
@@ -433,7 +469,7 @@ int main(int argc, char *argv[])
     gfault->SetFaultState(0,0,"Initialize.");
 
     gstrfusionmsg = xparam.GetParam("fusionmsg","fusion_gpslidar");
-    gstrgpsmsg = xparam.GetParam("gpsmsg","ins550d_gpsimu");
+    gstrgpsmsg = xparam.GetParam("gpsmsg","hcp2_gpsimu");
     gstrndtmsg = xparam.GetParam("ndtgpsposmsg","ndtgpspos");
     gstrlidaronly = xparam.GetParam("LidarOnly","false");
     gstrfusiongpsimumsg = xparam.GetParam("fusiongpsimumsg","fusion_gpsimu");
@@ -459,6 +495,7 @@ int main(int argc, char *argv[])
     gpa = iv::modulecomm::RegisterRecv(gstrgpsmsg.data(),ListenRaw);
     gpb = iv::modulecomm::RegisterRecv(gstrndtmsg.data(),ListenNDTGPS);
     gpd = iv::modulecomm::RegisterSend(gstrfusiongpsimumsg.data(),10000,1);
+    gpe = iv::modulecomm::RegisterRecv("chassis",UpdateChassis);
 
 
     std::cout<<"start fusion_gpsndt"<<std::endl;

src/fusion/fusion_pointcloud_shenlan/fusion_pointcloud_shenlan.pro

@@ -0,0 +1,96 @@
+QT -= gui
+
+CONFIG += c++14 #console
+CONFIG -= app_bundle
+
+# 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
+QMAKE_LFLAGS += -no-pie
+
+# 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 \
+    lidarmerge.cpp
+
+INCLUDEPATH += /usr/include/pcl-1.7
+INCLUDEPATH += /usr/include/pcl-1.8
+INCLUDEPATH += /usr/include/eigen3
+
+#QMAKE_CXXFLAGS +=  -g
+
+#QMAKE_CXXFLAGS +=  -g
+
+
+#LIBS += /usr/lib/aarch64-linux-gnu/libpcl_*.so
+
+win32: LIBS += -LC:/File/PCL_1.8/PCL_1.8.1/lib -lpcl_common_debug\
+        -lpcl_features_debug\
+        -lpcl_filters_debug\
+        -lpcl_io_debug\
+        -lpcl_io_ply_debug\
+        -lpcl_kdtree_debug\
+        -lpcl_keypoints_debug\
+        -lpcl_octree_debug\
+        -lpcl_outofcore_debug\
+        -lpcl_people_debug\
+        -lpcl_recognition_debug\
+        -lpcl_registration_debug\
+        -lpcl_sample_consensus_debug\
+        -lpcl_search_debug\
+        -lpcl_segmentation_debug\
+        -lpcl_surface_debug\
+        -lpcl_tracking_debug\
+        -lpcl_visualization_debug
+
+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
+
+HEADERS += \
+    lidarmerge.h
+
+
+unix:LIBS += -lboost_thread -lboost_system
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+!include(../../../include/ivyaml-cpp.pri ) {
+    error( "Couldn't find the ivyaml-cpp.pri file!" )
+}
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+
+INCLUDEPATH += /usr/include/eigen3

src/fusion/fusion_pointcloud_shenlan/fusion_pointcloud_shenlan.yaml

@@ -0,0 +1,24 @@
+limitside: false  # if true ,use maximm minimin limit left right lidar.
+left:
+  memname: lidarpc_left
+  maximum:
+    x: 100
+    y: 100.0
+    z: 100
+  minimum:
+    x: -100
+    y: -100.0
+    z: -100
+right:
+  memname: lidarpc_right
+  maximum:
+    x: 100
+    y: 100.0
+    z: 100
+  minimum:
+    x: -100
+    y: -100.0
+    z: -100
+center:
+  memname: lidar_pc
+output: lidarpc_fusion

src/fusion/fusion_pointcloud_shenlan/lidarmerge.cpp

@@ -0,0 +1,74 @@
+#include "lidarmerge.h"
+
+#include <QDateTime>
+
+#include <QMutex>
+
+extern QMutex gMutex;
+
+static int g_seq = 0;
+
+pcl::PointCloud<pcl::PointXYZI>::Ptr mergefunc(std::vector<iv::lidar_data> xvectorlidar)
+{
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
+    point_cloud->header.frame_id = "velodyne";
+    point_cloud->height = 1;
+    point_cloud->header.stamp = QDateTime::currentMSecsSinceEpoch();
+    point_cloud->width = 0;
+    point_cloud->header.seq =g_seq;
+    g_seq++;
+
+
+    int i,j;
+    for(i=0;i<xvectorlidar.size();i++)
+    {
+        int nsize;
+
+        pcl::PointCloud<pcl::PointXYZI>::Ptr partpoint_cloud;
+        int64_t curtime = QDateTime::currentMSecsSinceEpoch();
+        if((xvectorlidar[i].bUpdate)||((curtime -xvectorlidar[i].mupdatetime)<150))
+        {
+            gMutex.lock();
+            partpoint_cloud = xvectorlidar[i].mpoint_cloud;
+            gMutex.unlock();
+            nsize = partpoint_cloud->width;
+            qDebug("size is %d ",nsize);
+
+            for(j=0;j<nsize;j++)
+            {
+                pcl::PointXYZI point;
+                point = partpoint_cloud->at(j);
+                point.x = (point.x + xvectorlidar[i].foff_x)*cos(xvectorlidar[i].foff_angle) -(point.y + xvectorlidar[i].foff_y)*sin(xvectorlidar[i].foff_angle) ;
+                point.y = (point.y + xvectorlidar[i].foff_y)*cos(xvectorlidar[i].foff_angle) + (point.x + xvectorlidar[i].foff_x)*sin(xvectorlidar[i].foff_angle);
+                point.z = point.z + xvectorlidar[i].foff_z;
+
+                if(point.x>xvectorlidar[i].fmax_x)continue;
+                if(point.y>xvectorlidar[i].fmax_y)continue;
+                if(point.z>xvectorlidar[i].fmax_z)continue;
+                if(point.z > xvectorlidar[i].fignore_zmax) continue;
+                if(point.z < xvectorlidar[i].fignore_zmin) continue;
+//                std::cout<<"  fignore  zmin  "<<xvectorlidar[i].fignore_zmin<<std::endl;
+
+                if(point.x<xvectorlidar[i].fmin_x)continue;
+                if(point.y<xvectorlidar[i].fmin_y)continue;
+                if(point.z<xvectorlidar[i].fmin_z)continue;
+
+                if((point.x>xvectorlidar[i].fignore_xmin)&&(point.x<xvectorlidar[i].fignore_xmax)&&(point.y>xvectorlidar[i].fignore_ymin)&&(point.y<xvectorlidar[i].fignore_ymax))
+                {
+                    continue;
+                }
+    //            if(!pcl_isfinite(point.x)||!pcl_isfinite(point.y)||!pcl_isfinite(point.z)||!pcl_isfinite(point.intensity)) continue;
+
+                point_cloud->push_back(point);
+                ++point_cloud->width;
+            }
+
+
+        }
+    }
+    qDebug("merge size is %d ",point_cloud->width);
+
+    return point_cloud;
+}

src/fusion/fusion_pointcloud_shenlan/lidarmerge.h

@@ -0,0 +1,46 @@
+#ifndef LIDARMERGE_H
+#define LIDARMERGE_H
+
+#include <pcl/common/io.h>
+
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+
+#include <mutex>
+
+
+
+namespace  iv {
+struct lidar_data
+{
+    std::mutex mmutex;
+    char strmemname[256];
+
+
+
+    double foff_x = 0.0;
+    double foff_y = 0.0;
+    double foff_z = 0.0;
+    double foff_angle = 0.0;
+    bool bUpdate = false;
+    double fmax_x = 0;
+    double fmax_y = 0;
+    double fmax_z = 0;
+    double fmin_x = 0;
+    double fmin_y = 0;
+    double fmin_z = 0;
+    double fignore_xmax = 0;
+    double fignore_ymax = 0;
+    double fignore_xmin = 0;
+    double fignore_ymin = 0;
+    double fignore_zmax = 0;
+    double fignore_zmin = 0;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud = NULL;
+    int64_t mupdatetime = 0;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud_old = NULL;
+};
+
+}
+
+pcl::PointCloud<pcl::PointXYZI>::Ptr mergefunc(std::vector<iv::lidar_data> xvectorlidar);
+#endif // LIDARMERGE_H

src/fusion/fusion_pointcloud_shenlan/main.cpp

@@ -0,0 +1,451 @@
+#include <QCoreApplication>
+#include <QDateTime>
+
+#include <pcl/common/io.h>
+
+
+#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+//#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/filters/radius_outlier_removal.h>
+
+#include <iostream>
+#include <fstream>
+#include <yaml-cpp/yaml.h>
+#include <QMutex>
+#include <vector>
+#include <thread>
+
+#include <pcl/common/transforms.h>
+
+
+#include <Eigen/Eigen>
+
+#include "lidarmerge.h"
+#include "modulecomm.h"
+#include "ivversion.h"
+
+
+QMutex gMutex;
+
+static qint64 glasttime = 0;
+static std::vector<iv::lidar_data> gvectorlidar;
+
+static iv::lidar_data glidar_data_left;
+static iv::lidar_data glidar_data_right;
+static iv::lidar_data glidar_data_center;
+
+static char gstrcentermemename[256];
+static char gstroutmemname[256];
+static void * gpaout;
+
+static bool gbrun = true;
+
+static Eigen::Matrix4f gleft_calib(Eigen::Matrix4f::Identity());
+static Eigen::Matrix4f gright_calib(Eigen::Matrix4f::Identity());
+
+static bool gbLoadLeftCalib = false;
+static bool gbLoadRightCalib = false;
+static bool gbLimitSide = false; //if true, use yaml max min
+
+static void InitLidarData()
+{
+    snprintf(glidar_data_left.strmemname,256,"lidarpc_left");
+    snprintf(glidar_data_right.strmemname,256,"lidarpc_right");
+
+    glidar_data_left.fmax_x = 300;
+    glidar_data_left.fmax_y = 300;
+    glidar_data_left.fmax_z = 300;
+    glidar_data_left.fmin_x = -300;
+    glidar_data_left.fmin_y = -300;
+    glidar_data_left.fmin_z = -300;
+    glidar_data_left.bUpdate = false;
+    glidar_data_left.mupdatetime = 0;
+
+    glidar_data_right.fmax_x = 300;
+    glidar_data_right.fmax_y = 300;
+    glidar_data_right.fmax_z = 300;
+    glidar_data_right.fmin_x = -300;
+    glidar_data_right.fmin_y = -300;
+    glidar_data_right.fmin_z = -300;
+    glidar_data_right.bUpdate = false;
+    glidar_data_right.mupdatetime = 0;
+
+    glidar_data_center.bUpdate = false;
+    glidar_data_center.mupdatetime = 0;
+
+}
+
+void dec_calibyaml(const char * stryamlpath,Eigen::Matrix4f & calib,bool & bload)
+{
+    YAML::Node config;
+    try
+    {
+        config = YAML::LoadFile(stryamlpath);
+    }
+    catch(YAML::BadFile e)
+    {
+        qDebug("load %s error.",stryamlpath);
+        return;
+    }
+
+
+
+    if(config["Calib"])
+    {
+        YAML::Node node_calib = config["Calib"];
+//        bool isscalar = node_calib.IsScalar();
+//        bool ismap = node_calib.IsMap();
+        bool isseq = node_calib.IsSequence();
+        if(isseq)
+        {
+            int size = node_calib.size();
+            int i=0;
+            int j= 0;
+            if(size == 16)
+            {
+                auto node_it = node_calib.begin();
+                for(;node_it != node_calib.end();node_it++)
+                {
+                    std::string strvalue = node_it->as<std::string>();
+ //                   std::cout<<"value:["<<strvalue<<"]"<<std::endl;
+                    calib(i,j) = atof(strvalue.data());
+                    j++;
+                    if(j>=4)
+                    {
+                        i++;
+                        j=0;
+                    }
+                }
+                bload = true;
+            }
+            else
+            {
+                std::cout<<" calib not 16 values. "<<std::endl;
+            }
+        }
+        else
+        {
+            std::cout<<" calib value not seq. "<<std::endl;
+        }
+
+    }
+
+
+
+}
+
+void dec_yaml(const char * stryamlpath)
+{
+
+    YAML::Node config;
+    try
+    {
+        config = YAML::LoadFile(stryamlpath);
+    }
+    catch(YAML::BadFile e)
+    {
+        qDebug("load %s error.",stryamlpath);
+        return;
+    }
+
+    if(config["limitside"])
+    {
+        std::string strlimit = config["limitside"].as<std::string>();
+        if(strlimit == "true")
+        {
+            gbLimitSide = true;
+        }
+    }
+
+    if(config["left"]["memname"])
+    {
+        snprintf(glidar_data_left.strmemname,256,"%s",config["left"]["memname"].as<std::string>().data());
+    }
+    if(config["left"]["maximum"]["x"])glidar_data_left.fmax_x = atof(config["left"]["maximum"]["x"].as<std::string>().data());
+    if(config["left"]["maximum"]["y"])glidar_data_left.fmax_y = atof(config["left"]["maximum"]["y"].as<std::string>().data());
+    if(config["left"]["maximum"]["z"])glidar_data_left.fmax_z = atof(config["left"]["maximum"]["z"].as<std::string>().data());
+    if(config["left"]["minimum"]["x"])glidar_data_left.fmin_x = atof(config["left"]["minimum"]["x"].as<std::string>().data());
+    if(config["left"]["minimum"]["y"])glidar_data_left.fmin_y = atof(config["left"]["minimum"]["y"].as<std::string>().data());
+    if(config["left"]["minimum"]["z"])glidar_data_left.fmin_z = atof(config["left"]["minimum"]["z"].as<std::string>().data());
+
+    if(config["right"]["memname"])
+    {
+        snprintf(glidar_data_right.strmemname,256,"%s",config["right"]["memname"].as<std::string>().data());
+    }
+    if(config["right"]["maximum"]["x"])glidar_data_right.fmax_x = atof(config["right"]["maximum"]["x"].as<std::string>().data());
+    if(config["right"]["maximum"]["y"])glidar_data_right.fmax_y = atof(config["right"]["maximum"]["y"].as<std::string>().data());
+    if(config["right"]["maximum"]["z"])glidar_data_right.fmax_z = atof(config["right"]["maximum"]["z"].as<std::string>().data());
+    if(config["right"]["minimum"]["x"])glidar_data_right.fmin_x = atof(config["right"]["minimum"]["x"].as<std::string>().data());
+    if(config["right"]["minimum"]["y"])glidar_data_right.fmin_y = atof(config["right"]["minimum"]["y"].as<std::string>().data());
+    if(config["right"]["minimum"]["z"])glidar_data_right.fmin_z = atof(config["right"]["minimum"]["z"].as<std::string>().data());
+
+    if(config["center"]["memname"])
+    {
+        snprintf(gstrcentermemename,256,"%s",config["center"]["memname"].as<std::string>().data());
+    }
+
+
+    if(config["output"])
+    {
+       strncpy(gstroutmemname,config["output"].as<std::string>().data(),255);
+    }
+    else
+    {
+        strncpy(gstroutmemname,"lidar_pc",255);
+    }
+
+    iv::lidar_data * pl = &glidar_data_left;
+    pl = &glidar_data_right;
+
+    char * strmemc = gstrcentermemename;
+    strmemc = gstroutmemname;
+
+    return;
+
+}
+
+void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+//    std::cout<<" listen point cloud. "<<std::endl;
+    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::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+    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::PointXYZI xp;
+        xp.x = p->x;
+        xp.y = p->y;
+        xp.z = p->z;
+        xp.intensity = p->intensity;
+        point_cloud->push_back(xp);
+        p++;
+
+  //      std::cout<<" x is "<<xp.x<<" y is "<<xp.y<<std::endl;
+    }
+
+    if(strncmp(strmemname,glidar_data_left.strmemname,255) == 0)
+    {
+        glidar_data_left.mmutex.lock();
+        glidar_data_left.mpoint_cloud = point_cloud;
+        glidar_data_left.mupdatetime = QDateTime::currentMSecsSinceEpoch();
+        glidar_data_left.bUpdate = true;
+        glidar_data_left.mmutex.unlock();
+    }
+
+    if(strncmp(strmemname,glidar_data_right.strmemname,255) == 0)
+    {
+        glidar_data_right.mmutex.lock();
+        glidar_data_right.mpoint_cloud = point_cloud;
+        glidar_data_right.mupdatetime = QDateTime::currentMSecsSinceEpoch();
+        glidar_data_right.bUpdate = true;
+        glidar_data_right.mmutex.unlock();
+    }
+
+    if(strncmp(strmemname,gstrcentermemename,255) == 0)
+    {
+        glidar_data_center.mmutex.lock();
+        glidar_data_center.mpoint_cloud = point_cloud;
+        glidar_data_center.mupdatetime = QDateTime::currentMSecsSinceEpoch();
+        glidar_data_center.bUpdate = true;
+        glidar_data_center.mmutex.unlock();
+    }
+
+}
+
+void sharepointcloud(pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud,void * pa)
+{
+    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI)];
+
+    int * pHeadSize = (int *)strOut;
+    *pHeadSize = 4 + point_cloud->header.frame_id.size()+4+8;
+    memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
+  //  pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+    unsigned int * pu32 = (unsigned int *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+    *pu32 = point_cloud->header.seq;
+    memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)(strOut +4+sizeof(point_cloud->header.frame_id.size()) + 4+8 );
+    memcpy(p,point_cloud->points.data(),point_cloud->width * sizeof(pcl::PointXYZI));
+
+    iv::modulecomm::ModuleSendMsg(pa,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
+    delete strOut;
+}
+
+
+void LimitSide(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud,iv::lidar_data & xlidardata)
+{
+    pcl::PointCloud<pcl::PointXYZI>::Ptr new_scan(new pcl::PointCloud<pcl::PointXYZI>());
+    new_scan->header = point_cloud->header;
+    new_scan->width = 0;
+
+    pcl::PointXYZI p;
+    for (pcl::PointCloud<pcl::PointXYZI>::const_iterator item = point_cloud->begin(); item != point_cloud->end(); item++)
+    {
+      p.x = (double)item->x;
+      if(p.x > xlidardata.fmax_x)continue;
+      if(p.x < xlidardata.fmin_x)continue;
+
+      p.y = (double)item->y;
+      if(p.y > xlidardata.fmax_y)continue;
+      if(p.y < xlidardata.fmin_y)continue;
+
+      p.z = (double)item->z;
+      if(p.z > xlidardata.fmax_z)continue;
+      if(p.z < xlidardata.fmin_z)continue;
+
+      p.intensity = (double)item->intensity;
+
+      new_scan->push_back(p);
+      new_scan->width++;
+
+    }
+
+    point_cloud = new_scan;
+
+}
+
+
+void mergethread()
+{
+    int i;
+    int64_t nleftupdatetime = 0;
+    int64_t nrightupdatetime = 0;
+    int64_t ncenterupdatetime = 0;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_left = NULL;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_right = NULL;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_center = NULL;
+    while(gbrun)
+    {
+        bool bCenterUpdate = false;
+        bool bLeftUpdate = false;
+        bool bRightUpdate = false;
+        glidar_data_left.mmutex.lock();
+        if(glidar_data_left.bUpdate)
+        {
+            point_cloud_left = glidar_data_left.mpoint_cloud;
+            nleftupdatetime = glidar_data_left.mupdatetime;
+            glidar_data_left.bUpdate = false;
+            bLeftUpdate = true;
+        }
+        glidar_data_left.mmutex.unlock();
+
+        glidar_data_right.mmutex.lock();
+        if(glidar_data_right.bUpdate)
+        {
+            point_cloud_right = glidar_data_right.mpoint_cloud;
+            nrightupdatetime = glidar_data_right.mupdatetime;
+            glidar_data_right.bUpdate = false;
+            bRightUpdate = true;
+        }
+        glidar_data_right.mmutex.unlock();
+
+        glidar_data_center.mmutex.lock();
+        if(glidar_data_center.bUpdate)
+        {
+            point_cloud_center = glidar_data_center.mpoint_cloud;
+            ncenterupdatetime = glidar_data_center.mupdatetime;
+            glidar_data_center.bUpdate = false;
+            bCenterUpdate = true;
+        }
+        glidar_data_center.mmutex.unlock();
+
+        if(bLeftUpdate && gbLoadLeftCalib)
+        {
+            if(gbLimitSide)
+            {
+                LimitSide(point_cloud_left,glidar_data_left);
+            }
+            pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_scan_ptr2(new pcl::PointCloud<pcl::PointXYZI>());
+            pcl::transformPointCloud(*point_cloud_left, *transformed_scan_ptr2, gleft_calib);
+            point_cloud_left = transformed_scan_ptr2;
+        }
+        if(bRightUpdate && gbLoadRightCalib)
+        {
+            if(gbLimitSide)
+            {
+                LimitSide(point_cloud_right,glidar_data_right);
+            }
+            pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_scan_ptr2(new pcl::PointCloud<pcl::PointXYZI>());
+            pcl::transformPointCloud(*point_cloud_right, *transformed_scan_ptr2, gright_calib);
+            point_cloud_right = transformed_scan_ptr2;
+        }
+        //transform
+
+        if(bCenterUpdate)
+        {
+            pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_out(new pcl::PointCloud<pcl::PointXYZI>());
+            point_cloud_out = point_cloud_center;
+            if(gbLoadLeftCalib &&(abs(ncenterupdatetime - nleftupdatetime)<200) &&(point_cloud_left != NULL))
+            {
+                *point_cloud_out = *point_cloud_out + *point_cloud_left;
+            }
+            else
+            {
+                std::cout<<" no left "<<" left calib: "<<gbLoadLeftCalib<<" time diff: "<<(ncenterupdatetime - nleftupdatetime)<<std::endl;
+            }
+            if(gbLoadRightCalib &&(abs(ncenterupdatetime - nrightupdatetime)<200) &&(point_cloud_right != NULL))
+            {
+                *point_cloud_out = *point_cloud_out + *point_cloud_right;
+            }
+            else
+            {
+                std::cout<<" no right "<<" right calib: "<<gbLoadRightCalib<<" time diff: "<<(ncenterupdatetime - nrightupdatetime)<<std::endl;
+            }
+            sharepointcloud(point_cloud_out,gpaout);
+        }
+        //Merge
+
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+
+    }
+}
+
+
+int main(int argc, char *argv[])
+{
+    showversion("fusion_pointcloud_shenlan");
+    QCoreApplication a(argc, argv);
+
+    InitLidarData();
+
+    snprintf(gstrcentermemename,256,"lidar_pc");
+    snprintf(gstroutmemname,256,"lidarpc_fusion");
+
+    dec_yaml("fusion_pointcloud_shenlan.yaml");
+    dec_calibyaml("fusion_left.yaml",gleft_calib,gbLoadLeftCalib);
+    dec_calibyaml("fusion_right.yaml",gright_calib,gbLoadRightCalib);
+
+    std::cout<<"left calib: \n[ "<<gleft_calib<<" ]"<<std::endl;
+    std::cout<<"right calib: \n[ "<<gright_calib<<" ]"<<std::endl;
+
+    iv::modulecomm::RegisterRecv(glidar_data_left.strmemname,ListenPointCloud);
+    iv::modulecomm::RegisterRecv(glidar_data_right.strmemname,ListenPointCloud);
+    iv::modulecomm::RegisterRecv(gstrcentermemename,ListenPointCloud);
+
+    gpaout = iv::modulecomm::RegisterSend(gstroutmemname,20000000,1);
+
+    std::thread xthread(mergethread);
+
+    (void)xthread;
+
+    return a.exec();
+}

src/fusion/lidar_radar_fusion_cnn/lidar_radar_fusion_cnn.pro

@@ -77,6 +77,7 @@ INCLUDEPATH += $$PWD/../../include/msgtype
 #}
 
 INCLUDEPATH += /usr/include/opencv4/
-LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so
+#LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so
 
 
+LIBS += /usr/lib/libopencv*.so

src/include/proto/groupctrl.proto

@@ -0,0 +1,12 @@
+syntax = "proto2";
+
+package iv.group;
+
+
+message groupctrl
+{
+	required double fGroupSpeed = 1;  //km/h
+	required double fFrontChange = 2;  //m
+	required double fRearChange = 3;  //m
+}
+

src/tool/adc_cantool/LICENSE

@@ -0,0 +1,340 @@
+                    GNU GENERAL PUBLIC LICENSE
+                       Version 2, June 1991
+
+ Copyright (C) 1989, 1991 Free Software Foundation, Inc., <http://fsf.org/>
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+                            Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+License is intended to guarantee your freedom to share and change free
+software--to make sure the software is free for all its users.  This
+General Public License applies to most of the Free Software
+Foundation's software and to any other program whose authors commit to
+using it.  (Some other Free Software Foundation software is covered by
+the GNU Lesser General Public License instead.)  You can apply it to
+your programs, too.
+
+  When we speak of free software, we are referring to freedom, not
+price.  Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+this service if you wish), that you receive source code or can get it
+if you want it, that you can change the software or use pieces of it
+in new free programs; and that you know you can do these things.
+
+  To protect your rights, we need to make restrictions that forbid
+anyone to deny you these rights or to ask you to surrender the rights.
+These restrictions translate to certain responsibilities for you if you
+distribute copies of the software, or if you modify it.
+
+  For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must give the recipients all the rights that
+you have.  You must make sure that they, too, receive or can get the
+source code.  And you must show them these terms so they know their
+rights.
+
+  We protect your rights with two steps: (1) copyright the software, and
+(2) offer you this license which gives you legal permission to copy,
+distribute and/or modify the software.
+
+  Also, for each author's protection and ours, we want to make certain
+that everyone understands that there is no warranty for this free
+software.  If the software is modified by someone else and passed on, we
+want its recipients to know that what they have is not the original, so
+that any problems introduced by others will not reflect on the original
+authors' reputations.
+
+  Finally, any free program is threatened constantly by software
+patents.  We wish to avoid the danger that redistributors of a free
+program will individually obtain patent licenses, in effect making the
+program proprietary.  To prevent this, we have made it clear that any
+patent must be licensed for everyone's free use or not licensed at all.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.
+
+                    GNU GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License applies to any program or other work which contains
+a notice placed by the copyright holder saying it may be distributed
+under the terms of this General Public License.  The "Program", below,
+refers to any such program or work, and a "work based on the Program"
+means either the Program or any derivative work under copyright law:
+that is to say, a work containing the Program or a portion of it,
+either verbatim or with modifications and/or translated into another
+language.  (Hereinafter, translation is included without limitation in
+the term "modification".)  Each licensee is addressed as "you".
+
+Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running the Program is not restricted, and the output from the Program
+is covered only if its contents constitute a work based on the
+Program (independent of having been made by running the Program).
+Whether that is true depends on what the Program does.
+
+  1. You may copy and distribute verbatim copies of the Program's
+source code as you receive it, in any medium, provided that you
+conspicuously and appropriately publish on each copy an appropriate
+copyright notice and disclaimer of warranty; keep intact all the
+notices that refer to this License and to the absence of any warranty;
+and give any other recipients of the Program a copy of this License
+along with the Program.
+
+You may charge a fee for the physical act of transferring a copy, and
+you may at your option offer warranty protection in exchange for a fee.
+
+  2. You may modify your copy or copies of the Program or any portion
+of it, thus forming a work based on the Program, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) You must cause the modified files to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    b) You must cause any work that you distribute or publish, that in
+    whole or in part contains or is derived from the Program or any
+    part thereof, to be licensed as a whole at no charge to all third
+    parties under the terms of this License.
+
+    c) If the modified program normally reads commands interactively
+    when run, you must cause it, when started running for such
+    interactive use in the most ordinary way, to print or display an
+    announcement including an appropriate copyright notice and a
+    notice that there is no warranty (or else, saying that you provide
+    a warranty) and that users may redistribute the program under
+    these conditions, and telling the user how to view a copy of this
+    License.  (Exception: if the Program itself is interactive but
+    does not normally print such an announcement, your work based on
+    the Program is not required to print an announcement.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Program,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Program, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Program.
+
+In addition, mere aggregation of another work not based on the Program
+with the Program (or with a work based on the Program) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may copy and distribute the Program (or a work based on it,
+under Section 2) in object code or executable form under the terms of
+Sections 1 and 2 above provided that you also do one of the following:
+
+    a) Accompany it with the complete corresponding machine-readable
+    source code, which must be distributed under the terms of Sections
+    1 and 2 above on a medium customarily used for software interchange; or,
+
+    b) Accompany it with a written offer, valid for at least three
+    years, to give any third party, for a charge no more than your
+    cost of physically performing source distribution, a complete
+    machine-readable copy of the corresponding source code, to be
+    distributed under the terms of Sections 1 and 2 above on a medium
+    customarily used for software interchange; or,
+
+    c) Accompany it with the information you received as to the offer
+    to distribute corresponding source code.  (This alternative is
+    allowed only for noncommercial distribution and only if you
+    received the program in object code or executable form with such
+    an offer, in accord with Subsection b above.)
+
+The source code for a work means the preferred form of the work for
+making modifications to it.  For an executable work, complete source
+code means all the source code for all modules it contains, plus any
+associated interface definition files, plus the scripts used to
+control compilation and installation of the executable.  However, as a
+special exception, the source code distributed need not include
+anything that is normally distributed (in either source or binary
+form) with the major components (compiler, kernel, and so on) of the
+operating system on which the executable runs, unless that component
+itself accompanies the executable.
+
+If distribution of executable or object code is made by offering
+access to copy from a designated place, then offering equivalent
+access to copy the source code from the same place counts as
+distribution of the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  4. You may not copy, modify, sublicense, or distribute the Program
+except as expressly provided under this License.  Any attempt
+otherwise to copy, modify, sublicense or distribute the Program is
+void, and will automatically terminate your rights under this License.
+However, parties who have received copies, or rights, from you under
+this License will not have their licenses terminated so long as such
+parties remain in full compliance.
+
+  5. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Program or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Program (or any work based on the
+Program), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Program or works based on it.
+
+  6. Each time you redistribute the Program (or any work based on the
+Program), the recipient automatically receives a license from the
+original licensor to copy, distribute or modify the Program subject to
+these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties to
+this License.
+
+  7. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Program at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Program by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Program.
+
+If any portion of this section is held invalid or unenforceable under
+any particular circumstance, the balance of the section is intended to
+apply and the section as a whole is intended to apply in other
+circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system, which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  8. If the distribution and/or use of the Program is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Program under this License
+may add an explicit geographical distribution limitation excluding
+those countries, so that distribution is permitted only in or among
+countries not thus excluded.  In such case, this License incorporates
+the limitation as if written in the body of this License.
+
+  9. The Free Software Foundation may publish revised and/or new versions
+of the General Public License from time to time.  Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Program
+specifies a version number of this License which applies to it and "any
+later version", you have the option of following the terms and conditions
+either of that version or of any later version published by the Free
+Software Foundation.  If the Program does not specify a version number of
+this License, you may choose any version ever published by the Free Software
+Foundation.
+
+  10. If you wish to incorporate parts of the Program into other free
+programs whose distribution conditions are different, write to the author
+to ask for permission.  For software which is copyrighted by the Free
+Software Foundation, write to the Free Software Foundation; we sometimes
+make exceptions for this.  Our decision will be guided by the two goals
+of preserving the free status of all derivatives of our free software and
+of promoting the sharing and reuse of software generally.
+
+                            NO WARRANTY
+
+  11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
+FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW.  EXCEPT WHEN
+OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
+PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
+OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE ENTIRE RISK AS
+TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU.  SHOULD THE
+PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
+REPAIR OR CORRECTION.
+
+  12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
+REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
+INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
+OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
+TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
+YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
+PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGES.
+
+                     END OF TERMS AND CONDITIONS
+
+            How to Apply These Terms to Your New Programs
+
+  If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+  To do so, attach the following notices to the program.  It is safest
+to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+    {description}
+    Copyright (C) {year}  {fullname}
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License along
+    with this program; if not, write to the Free Software Foundation, Inc.,
+    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+
+Also add information on how to contact you by electronic and paper mail.
+
+If the program is interactive, make it output a short notice like this
+when it starts in an interactive mode:
+
+    Gnomovision version 69, Copyright (C) year name of author
+    Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+    This is free software, and you are welcome to redistribute it
+    under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License.  Of course, the commands you use may
+be called something other than `show w' and `show c'; they could even be
+mouse-clicks or menu items--whatever suits your program.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the program, if
+necessary.  Here is a sample; alter the names:
+
+  Yoyodyne, Inc., hereby disclaims all copyright interest in the program
+  `Gnomovision' (which makes passes at compilers) written by James Hacker.
+
+  {signature of Ty Coon}, 1 April 1989
+  Ty Coon, President of Vice
+
+This General Public License does not permit incorporating your program into
+proprietary programs.  If your program is a subroutine library, you may
+consider it more useful to permit linking proprietary applications with the
+library.  If this is what you want to do, use the GNU Lesser General
+Public License instead of this License.
+

src/tool/adc_cantool/adc_cantool.pro

@@ -0,0 +1,79 @@
+#-------------------------------------------------
+#
+# Project created by QtCreator 2023-01-12T09:44:18
+#
+#-------------------------------------------------
+lessThan(QT_MAJOR_VERSION, 5): error("requires Qt 5")
+QT       += core gui
+
+greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
+
+QT += charts
+QT += network
+QT += xml
+QT += charts
+QT += serialport
+
+
+CONFIG += ordered warn_on qt debug_and_release
+CONFIG += c++11
+
+TARGET = adc_cantool_driverEnable
+TEMPLATE = app
+QMAKE_LFLAGS += -no-pie
+
+CONFIG += warn_on
+CONFIG += link_pkgconfig
+
+#DESTDIR = ./bin
+#MOC_DIR = ./build/moc
+#RCC_DIR = ./build/rcc
+#UI_DIR = ./build/ui
+unix:OBJECTS_DIR = ./build/o/unix
+win32:OBJECTS_DIR = ./build/o/win32
+macx:OBJECTS_DIR = ./build/o/mac
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which has been marked as 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 \
+        mainwindow.cpp
+
+HEADERS += \
+        mainwindow.h
+
+FORMS += \
+        mainwindow.ui
+
+include($$PWD/core/core.pri)
+include($$PWD/driver/driver.pri)
+include($$PWD/parser/dbc/dbc.pri)
+include($$PWD/window/TraceWindow/TraceWindow.pri)
+include($$PWD/window/SetupDialog/SetupDialog.pri)
+include($$PWD/window/LogWindow/LogWindow.pri)
+include($$PWD/window/GraphWindow/GraphWindow.pri)
+include($$PWD/window/CanStatusWindow/CanStatusWindow.pri)
+include($$PWD/window/RawTxWindow/RawTxWindow.pri)
+include($$PWD/window/ChassisAnalysisWindow/ChassisAnalysisWindow.pri)
+#include($$PWD/window/ShenLanChassisAnalysisWindow/ShenLanChassisAnalysisWindow.pri)
+
+unix:PKGCONFIG += libnl-3.0
+unix:PKGCONFIG += libnl-route-3.0
+unix:include($$PWD/driver/SocketCanDriver/SocketCanDriver.pri)
+
+include($$PWD/driver/CANBlastDriver/CANBlastDriver.pri)
+include($$PWD/driver/SLCANDriver/SLCANDriver.pri)
+
+win32:include($$PWD/driver/CandleApiDriver/CandleApiDriver.pri)
+
+RESOURCES += \
+    cantool.qrc

src/tool/adc_cantool/assets/cangaroo.svg

@@ -0,0 +1,129 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<!-- Created with Inkscape (http://www.inkscape.org/) -->
+
+<svg
+   xmlns:dc="http://purl.org/dc/elements/1.1/"
+   xmlns:cc="http://creativecommons.org/ns#"
+   xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+   xmlns:svg="http://www.w3.org/2000/svg"
+   xmlns="http://www.w3.org/2000/svg"
+   xmlns:xlink="http://www.w3.org/1999/xlink"
+   xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
+   xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
+   width="200mm"
+   height="200mm"
+   viewBox="0 0 708.66142 708.66141"
+   id="svg2"
+   version="1.1"
+   inkscape:version="0.91 r13725"
+   sodipodi:docname="sign.svg">
+  <defs
+     id="defs4">
+    <linearGradient
+       inkscape:collect="always"
+       id="linearGradient4828">
+      <stop
+         style="stop-color:#ffe2a0;stop-opacity:1;"
+         offset="0"
+         id="stop4830" />
+      <stop
+         style="stop-color:#ffe2a0;stop-opacity:0;"
+         offset="1"
+         id="stop4832" />
+    </linearGradient>
+    <radialGradient
+       inkscape:collect="always"
+       xlink:href="#linearGradient4828"
+       id="radialGradient4834"
+       cx="-213.18985"
+       cy="735.4455"
+       fx="-213.18985"
+       fy="735.4455"
+       r="209.90335"
+       gradientUnits="userSpaceOnUse" />
+  </defs>
+  <sodipodi:namedview
+     id="base"
+     pagecolor="#ffffff"
+     bordercolor="#666666"
+     borderopacity="1.0"
+     inkscape:pageopacity="0.0"
+     inkscape:pageshadow="2"
+     inkscape:zoom="0.98994949"
+     inkscape:cx="386.90375"
+     inkscape:cy="377.91498"
+     inkscape:document-units="px"
+     inkscape:current-layer="layer1"
+     showgrid="false"
+     showguides="true"
+     inkscape:guide-bbox="true"
+     inkscape:window-width="1920"
+     inkscape:window-height="1020"
+     inkscape:window-x="0"
+     inkscape:window-y="0"
+     inkscape:window-maximized="1" />
+  <metadata
+     id="metadata7">
+    <rdf:RDF>
+      <cc:Work
+         rdf:about="">
+        <dc:format>image/svg+xml</dc:format>
+        <dc:type
+           rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
+        <dc:title />
+      </cc:Work>
+    </rdf:RDF>
+  </metadata>
+  <g
+     inkscape:label="Ebene 1"
+     inkscape:groupmode="layer"
+     id="layer1"
+     transform="translate(0,-343.70079)">
+    <rect
+       style="fill:none;fill-opacity:1;stroke:none;stroke-width:8.30000019;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+       id="rect4234"
+       width="2006.163"
+       height="1337.442"
+       x="-719.22864"
+       y="-0.21672098"
+       ry="16.6278" />
+    <g
+       id="g4836"
+       inkscape:export-xdpi="40.310001"
+       inkscape:export-ydpi="40.310001">
+      <rect
+         transform="matrix(0.70710678,-0.70710678,0.70710678,0.70710678,0,0)"
+         style="fill:#eda800;fill-opacity:1;stroke:none;stroke-width:8.32127285;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+         id="rect4192"
+         width="419.8067"
+         height="419.8067"
+         x="-443.45035"
+         y="545.185"
+         ry="26.729334" />
+      <rect
+         transform="matrix(0.70710678,-0.70710678,0.70710678,0.70710678,0,0)"
+         ry="15.630058"
+         y="557.78033"
+         x="-430.85504"
+         height="394.61609"
+         width="394.61609"
+         id="rect4190"
+         style="fill:none;fill-opacity:1;stroke:#000000;stroke-width:8.61946106;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1" />
+      <rect
+         ry="26.729334"
+         y="545.185"
+         x="-443.45035"
+         height="419.8067"
+         width="419.8067"
+         id="rect4802"
+         style="fill:url(#radialGradient4834);fill-opacity:1;stroke:none;stroke-width:8.32127285;stroke-linecap:round;stroke-linejoin:round;stroke-miterlimit:4;stroke-dasharray:none;stroke-opacity:1"
+         transform="matrix(0.70710678,-0.70710678,0.70710678,0.70710678,0,0)" />
+      <path
+         sodipodi:nodetypes="cccccccccccccccccccccccccccccccccc"
+         inkscape:connector-curvature="0"
+         id="path4147"
+         d="m 149.37797,666.06055 c 36.85202,68.27225 98.31598,9.21863 133.45962,-34.1469 53.33162,-69.83408 100.80365,-71.32307 143.01904,-20.61564 10.87924,12.85652 19.30216,26.25099 26.12101,28.11203 8.87164,0.39217 17.64483,10.43161 24.89623,1.61237 1.83635,-1.92185 4.31807,-4.73544 12.52974,-8.50183 5.52361,-1.27938 -19.63483,-5.89752 -24.59044,-19.08144 -1.12994,-3.53833 -0.11983,-6.185 6.58541,-2.6688 14.34879,1.96546 20.95743,7.42176 28.41231,16.97406 5.04585,-3.69521 16.76491,5.70699 18.97359,8.13619 14.75199,6.79698 24.03482,14.12345 26.40937,18.02486 4.01044,5.23554 -0.89719,9.98884 -10.17097,7.32428 -12.26094,0.096 -12.0494,4.0172 -33.13885,3.449 -3.45976,5.64441 -4.23485,10.65196 -22.89809,22.23524 -3.14976,5.5128 -4.58537,17.95946 -27.94172,24.09806 l -26.0148,37.41813 c -9.19812,12.57436 -25.78056,-3.7517 -12.11947,-13.99947 -2.36604,8.63596 3.75645,16.93693 6.88137,7.53009 -0.24723,-18.16238 3.80973,-21.32505 5.02217,-37.9579 -11.44974,-0.95836 -9.2316,-2.61478 -14.02838,-4.46019 -2.96842,1.50841 -12.72011,15.18724 -15.0803,26.34009 7.49746,52.38941 -1.11195,57.51121 23.04091,61.65535 21.05587,5.39221 52.17215,14.12286 66.23052,20.37982 6.38897,-1.65469 11.18169,-0.78625 14.00807,2.71867 5.90335,0.80925 9.47167,3.0768 9.72272,6.22069 -4.88251,-2.946 -9.20347,-1.09013 -12.21099,2.02749 -13.90963,0.88066 -20.05007,-1.48264 -21.42356,-4.53159 -26.0187,-3.31013 -63.37293,-9.98546 -76.66616,-11.77218 -13.92805,0.17623 -13.35645,-11.68119 -14.69203,-17.00635 -8.44769,-29.03516 -17.45108,-48.6236 -26.58765,-65.94782 -0.95035,-1.81134 -1.41819,-3.62267 -1.63294,-5.43401 -28.26207,-3.09944 -38.91315,-27.2472 -58.24412,-43.13013 l -58.78012,32.78603 c -47.9789,29.96939 -93.14922,-9.54483 -89.09149,-37.7882 z"
+         style="fill:#000000;fill-opacity:1;fill-rule:evenodd;stroke:none;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" />
+    </g>
+  </g>
+</svg>