change adcndtmultimapping. add calib.

src/tool/adcndtmultimapping/main.cpp

@@ -40,5 +40,8 @@ int main(int argc, char *argv[])
     MainWindow w;
     w.show();
 
+ //   double a = asin(-0.1);
+    qDebug("asin(-0.1) = %f",asin(-0.1));
+
     return a.exec();
 }

src/tool/adcndtmultimapping/mainwindow.cpp

@@ -368,6 +368,8 @@ void MainWindow::onTimer()
         iv::ndttracepoint xtrrtk;
         while(mvectorrawgps.size()>0)
         {
+ //           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-15)))
             {
                 iv::ndttracepoint xtr;
@@ -421,6 +423,11 @@ void MainWindow::onTimer()
                      xtr.x,xtr.y,xtr.z,xtr.yaw);
             ui->lineEdit_Calib->setText(strtemp);
         }
+        if((gsplit_cur>50.0)&&(mbCalcCalib == false))
+        {
+            CalcCalib();
+            mbCalcCalib = true;
+        }
         update();
     }
     //记录执行了多少次
@@ -804,6 +811,15 @@ void MainWindow::CreateView()
 
     CreateParamView(gll1);
 
+    QGroupBox * gp2 = new QGroupBox(ui->centralWidget);
+    gp2->setTitle(QStringLiteral("Calib"));
+    gp2->setGeometry(QRect(10,730,430,150));
+    QGridLayout * gll2 = new QGridLayout(ui->centralWidget);
+
+    gp2->setLayout(gll2);
+
+    CreateCalibView(gll2);
+
 
 
     mptraceview = new ivtraceview();
@@ -905,6 +921,46 @@ void MainWindow::resizeEvent(QResizeEvent *event)
 
 
 }
+
+void MainWindow::CreateCalibView(QGridLayout * gl)
+{
+    gl->setSpacing(10);
+    int i = 0;
+
+    QLabel * pl = new QLabel(this);
+    pl->setText("Yaw");
+    pl->setFixedWidth(200);
+    QLineEdit * ple = new QLineEdit(this);
+    ple->setFixedWidth(100);
+    ple->setText(" ");
+    gl->addWidget(pl,i,0);
+    gl->addWidget(ple,i,1);
+    mpLE_CalcCalib_Yaw = ple;
+    i++;
+
+    pl = new QLabel(this);
+    pl->setText("Pitch");
+    pl->setFixedWidth(200);
+    ple = new QLineEdit(this);
+    ple->setFixedWidth(100);
+    ple->setText(" ");
+    gl->addWidget(pl,i,0);
+    gl->addWidget(ple,i,1);
+    mpLE_CalcCalib_Pitch = ple;
+    i++;
+
+    pl = new QLabel(this);
+    pl->setText("Roll");
+    pl->setFixedWidth(200);
+    ple = new QLineEdit(this);
+    ple->setFixedWidth(100);
+    ple->setText(" ");
+    gl->addWidget(pl,i,0);
+    gl->addWidget(ple,i,1);
+    mpLE_CalcCalib_Roll = ple;
+    i++;
+}
+
 //创建组合框内的Label和LineEdit，并设置初始化值
 void MainWindow::CreateParamView(QGridLayout *gl)
 {
@@ -1293,3 +1349,42 @@ void MainWindow::onSetArm()
     arm_y = mpLE_Army->text().toDouble();
 }
 
+void MainWindow::CalcCalib()
+{
+    if((mvectorgps.size()>0) &&(mvectortrace.size()>0))
+    {
+        double gps_x = mvectorgps[mvectorgps.size()-1].x;
+        double gps_y = mvectorgps[mvectorgps.size()-1].y;
+        double gps_z = mvectorgps[mvectorgps.size()-1].z;
+
+        double ndt_x = mvectortrace[mvectortrace.size()-1].x;
+        double ndt_y = mvectortrace[mvectortrace.size()-1].y;
+        double ndt_z = mvectortrace[mvectortrace.size()-1].z;
+
+        double gps_dis = sqrt(pow(gps_x,2)+pow(gps_y,2));
+        double ndt_dis = sqrt(pow(ndt_x,2)+pow(ndt_y,2));
+
+        if((gps_dis>0)&&(ndt_dis>0))
+        {
+            double gps_angle = asin(gps_y/gps_dis);
+            if(gps_x < 0)
+            {
+                gps_angle = M_PI - gps_angle;
+            }
+
+            double ndt_angle = asin(ndt_y/ndt_dis);
+            if(ndt_x < 0)
+            {
+                ndt_angle = M_PI - ndt_angle;
+            }
+
+            double calib_yaw = gps_angle - ndt_angle;
+            while(calib_yaw>2.0*M_PI) calib_yaw = calib_yaw - 2.0*M_PI;
+            while(calib_yaw < 0)calib_yaw = calib_yaw + 2.0*M_PI;
+
+            mpLE_CalcCalib_Yaw->setText(QString::number(calib_yaw*180.0/M_PI));
+        }
+
+    }
+}
+

src/tool/adcndtmultimapping/mainwindow.h

@@ -133,6 +133,8 @@ private:
 
     QDateTime mdtrpstart;
 
+    bool mbCalcCalib = false;
+
     inline QDateTime GetDateTimeFromRH(iv::RecordHead rh);
     inline bool ReadARecord(int & nRecSize,char ** pstrName, int * pnDataSize, char ** pstrData);
 
@@ -141,8 +143,11 @@ private:
 private:
     void CreateView();
     void CreateParamView(QGridLayout * gl);
+    void CreateCalibView(QGridLayout * gl);
     void UpdateMapParam();
 
+    void CalcCalib();
+
 public:
      void resizeEvent(QResizeEvent *event);
 
@@ -175,6 +180,10 @@ private:
     QLineEdit * mpLE_OriginHeight;
     QLineEdit * mpLE_OriginHeading;
 
+    QLineEdit * mpLE_CalcCalib_Pitch;
+    QLineEdit * mpLE_CalcCalib_Roll;
+    QLineEdit * mpLE_CalcCalib_Yaw;
+
 
     QPushButton * mpPB_ChangeParam;
     QPushButton * mpPB_Split;

src/tool/adcndtmultimapping/ndt_mapping.cpp

@@ -1133,6 +1133,7 @@ void point_ndtmapping(pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan)
   g_ostr << "-----------------------------------------------------------------" << std::endl;
 //  g_ostr << "Sequence number: " << input->header.seq << std::endl;
   g_ostr << "Sequence number: " << raw_scan->header.seq << std::endl;
+   g_ostr << "Sequence time: " << raw_scan->header.stamp << std::endl;
   g_ostr << "Number of scan points: " << scan_ptr->size() << " points." << std::endl;
   g_ostr << "Number of filtered scan points: " << filtered_scan_ptr->size() << " points." << std::endl;
   g_ostr << "transformed_scan_ptr: " << transformed_scan_ptr->points.size() << " points." << std::endl;
@@ -1154,7 +1155,7 @@ void point_ndtmapping(pcl::PointCloud<pcl::PointXYZI>::Ptr raw_scan)
   //显示打印的信息字符串
   gstr = g_ostr.str();
 
-  if(gbOldLidar)gbOldLidar = current_scan_time;
+  if(gbOldLidar)glidartime = current_scan_time;
   else
     glidartime = current_scan_time/1000;