change adcndtmultimapping. add rtk gauss.

src/tool/adcndtmultimapping/adcndtmultimapping.xml

@@ -2,5 +2,7 @@
 	<node name="adcndtmultimapping">
 		<param name="msg" value="lidar_pc" />
 		<param name="oldlidar" value="false" />
+		<param name="gpsmsg" value="hcp2_gpsimu" />
+		<param name="UseRTKGauss" value="false" />
 	</node>
 </xml>

src/tool/adcndtmultimapping/main.cpp

@@ -13,6 +13,8 @@ std::string gstrimumsg;
 
 bool gbOldLidar = false;
 
+bool gbUseRTKGauss = false;
+
 
 int main(int argc, char *argv[])
 {
@@ -37,6 +39,7 @@ int main(int argc, char *argv[])
    {
        gbOldLidar = true;
    }
+   gbUseRTKGauss = xp.GetParam("UseRTKGauss",false);
     MainWindow w;
     w.show();
 

src/tool/adcndtmultimapping/mainwindow.cpp

@@ -19,6 +19,11 @@ extern iv::ndtorigin gndtorigin;
 
 extern bool gbManualSave;
 
+extern iv::ndttracepoint grtktracepoint;
+extern std::mutex gmutexrtk;
+extern iv::ndttracepoint grtklasttracepoint;
+extern bool gbHaveRTK;
+
 #include <Eigen/Geometry>
 
 #include <tf/tf.h>
@@ -278,6 +283,17 @@ void MainWindow::on_pushButton_Load_clicked()
     QString str = QFileDialog::getOpenFileName(this,tr("Open file"),"",tr("Record File(*.ivd)"));
     if(str.isEmpty())return;
 
+
+
+    grtktracepoint.x = 0;
+    grtktracepoint.y = 0;
+    grtktracepoint.z = 0;
+    grtktracepoint.yaw = 0;
+    grtktracepoint.pitch = 0;
+    grtktracepoint.roll = 0;
+    grtklasttracepoint = grtktracepoint;
+    gbHaveRTK = false;
+
     mvectorcalib.clear();
     mvectorgps.clear();
     mvectorrawgps.clear();
@@ -393,9 +409,19 @@ void MainWindow::onTimer()
         iv::ndttracepoint xtrrtk;
         while(mvectorrawgps.size()>0)
         {
+            int64_t timediff = mvectorrawgps[0].recvtime - xlidartime;
  //           std::cout<<" time diff: "<<(mvectorrawgps[0].recvtime - xlidartime)<<std::endl;
  //           std::cout<<" ldiar time: "<<xlidartime<<std::endl;
-            if((mvectorrawgps[0].recvtime<xlidartime) &&(mvectorrawgps[0].recvtime>=(xlidartime-30)))
+            if((mvectorrawgps[0].recvtime<xlidartime) &&(mvectorrawgps[0].recvtime>=(xlidartime-15)))
+            {
+                iv::ndttracepoint xtr;
+                xtr = mvectorrawgps[0].xtrace;
+                mvectorgps.push_back(xtr);
+                bHaveRTKPoint = true;
+                xtrrtk = xtr;
+                break;
+            }
+            if((timediff>=0) &&(timediff < 15))
             {
                 iv::ndttracepoint xtr;
                 xtr = mvectorrawgps[0].xtrace;
@@ -405,6 +431,7 @@ void MainWindow::onTimer()
                 break;
             }
             mvectorrawgps.erase(mvectorrawgps.begin());
+
         }
 
         if(mbFileDiffOpen)
@@ -501,6 +528,7 @@ void MainWindow::onTimer()
                                     {
                                         if(xgpsimu.rtk_state() == 6)
                                         {
+                                            gbHaveRTK = true;
                                             double fheading = xgpsimu.heading();
                                             fheading = fheading - 0;
                                             if(fheading < 0)fheading = fheading + 360;
@@ -523,6 +551,7 @@ void MainWindow::onTimer()
                                             trarm.yaw = 0;
                                             trarm.pitch = 0;
                                             trarm.roll = 0;
+                                            std::cout<<" arm x: "<<arm_x<<" arm y: "<<arm_y<<std::endl;
                                             gndtorigin = PoseToGPS(gndtorigin,trarm);
 
                                             mpLE_OriginLat->setText(QString::number(gndtorigin.lat,'f',7));
@@ -539,6 +568,16 @@ void MainWindow::onTimer()
                                     if(mbIVDSetOrigin)
                                     {
                                         iv::ndttracepoint  xtr = CalcPose(gndtorigin,xgpsimu);
+
+
+                                        gmutexrtk.lock();
+                                        grtktracepoint.x = xtr.x;
+                                        grtktracepoint.y = xtr.y;
+                                        grtktracepoint.z = xtr.z;
+                                        grtktracepoint.yaw = xtr.yaw;
+                                        gmutexrtk.unlock();
+
+
                                         double x,y;
                                         x = arm_x * cos(xtr.yaw) - arm_y * sin(xtr.yaw);
                                         y = arm_x * sin(xtr.yaw) + arm_y * cos(xtr.yaw);

src/tool/adcndtmultimapping/mainwindow.h

@@ -234,7 +234,7 @@ private:
 
     bool mbFileDiffOpen = false;
 
-    double arm_x = 0.9;
+    double arm_x = 0.0;
     double arm_y = 0.0;
 
 

src/tool/adcndtmultimapping/ndt_mapping.cpp

@@ -214,6 +214,12 @@ bool gbManualSave = false;
 
 extern bool gbOldLidar;
 
+iv::ndttracepoint grtktracepoint;
+std::mutex gmutexrtk;
+iv::ndttracepoint grtklasttracepoint;
+bool gbHaveRTK;
+extern  bool gbUseRTKGauss;
+
 void setmethod(int nmethod)
 {
 
@@ -841,6 +847,29 @@ void point_ndtmapping(pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan)
   Eigen::Matrix4f init_guess =
       (init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix() * tf_btol;
 
+
+  iv::ndttracepoint xrtktrace;
+
+  gmutexrtk.lock();
+  xrtktrace = grtktracepoint;
+  gmutexrtk.unlock();
+
+  //根据预测的信息生成初始化的变换矩阵，即从初始状态变换到guess_pose_for_ndt状态的矩阵
+    Eigen::AngleAxisf rtk_rotation_x(xrtktrace.roll, Eigen::Vector3f::UnitX());
+    Eigen::AngleAxisf rtk_rotation_y(xrtktrace.pitch, Eigen::Vector3f::UnitY());
+    Eigen::AngleAxisf rtk_rotation_z(xrtktrace.yaw, Eigen::Vector3f::UnitZ());
+
+    Eigen::Translation3f rtk_translation(xrtktrace.x, xrtktrace.y, xrtktrace.z);
+
+    Eigen::Matrix4f rtk_guess =
+        (rtk_translation * rtk_rotation_z * rtk_rotation_y * rtk_rotation_x).matrix() * tf_btol;
+
+
+  if(gbUseRTKGauss && gbHaveRTK)
+  {
+    init_guess = rtk_guess;
+  }
+
 //  t3_end = ros::Time::now();
 //  d3 = t3_end - t3_start;
 

src/tool/adcndtmultimapping/ndt_mapping.h

@@ -7,6 +7,8 @@
 
 #include "ivlog.h"
 
+#include <mutex>
+
 namespace iv {
 struct ndttracepoint
 {