change decection_ndt_matching, fix pitch roll problem.

src/detection/detection_ndt_matching/detection_ndt_matching.pro

@@ -1,6 +1,6 @@
 QT -= gui
 QT += network
-CONFIG += console c++17
+CONFIG += c++17 #console
 CONFIG -= app_bundle
 
 QMAKE_CXXFLAGS += -std=gnu++17

src/detection/detection_ndt_matching/ndt_matching.cpp

@@ -332,11 +332,16 @@ 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);
 
+
+//    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);
@@ -387,8 +392,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);
@@ -647,6 +652,7 @@ void GPSPosMonitor(bool * pbRun)
             {
                 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;
@@ -1214,6 +1220,8 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
 
         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;
@@ -1267,6 +1275,7 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
 
 
     previous_pose = CalcPose(gvector_trace[gusemapindex].mndtorigin,gcurndtgpspos);
+ //   std::cout<<" previos head: "<<previous_pose.yaw<<std::endl;
     if(bNeedUpdateVel == true)
     {
         bNeedUpdateVel = false;
@@ -1439,9 +1448,12 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
 
 
 
-
+//      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);
@@ -1454,6 +1466,9 @@ void point_ndt(pcl::PointCloud<pcl::PointXYZ>::Ptr raw_scan)
 
  //     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)

src/detection/detection_ndt_matching_gpu_multi/ndt_matching.cpp

@@ -328,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);
 
@@ -383,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);