#include "mainwindow.h" #include "ui_mainwindow.h" #include #include #include #include "gpsimu.pb.h" #include "gnss_coordinate_convert.h" extern std::string gstrlidarmsg; extern std::string gstrimumsg; extern double gsplit_range ; extern double gsplit_filter; extern double gsplit_cur; extern iv::ndtorigin gndtorigin; extern bool gbManualSave; #include #include // Given two sets of 3D points, find the rotation + translation + scale // which best maps the first set to the second. // Source: http://en.wikipedia.org/wiki/Kabsch_algorithm // The input 3D points are stored as columns. Eigen::Affine3d Find3DAffineTransform(Eigen::Matrix3Xd in, Eigen::Matrix3Xd out) { // Default output Eigen::Affine3d A; A.linear() = Eigen::Matrix3d::Identity(3, 3); A.translation() = Eigen::Vector3d::Zero(); if (in.cols() != out.cols()) throw "Find3DAffineTransform(): input data mis-match"; // First find the scale, by finding the ratio of sums of some distances, // then bring the datasets to the same scale. double dist_in = 0, dist_out = 0; for (int col = 0; col < in.cols()-1; col++) { dist_in += (in.col(col+1) - in.col(col)).norm(); dist_out += (out.col(col+1) - out.col(col)).norm(); } if (dist_in <= 0 || dist_out <= 0) return A; double scale = dist_out/dist_in; out /= scale; // Find the centroids then shift to the origin Eigen::Vector3d in_ctr = Eigen::Vector3d::Zero(); Eigen::Vector3d out_ctr = Eigen::Vector3d::Zero(); for (int col = 0; col < in.cols(); col++) { in_ctr += in.col(col); out_ctr += out.col(col); } in_ctr /= in.cols(); out_ctr /= out.cols(); for (int col = 0; col < in.cols(); col++) { in.col(col) -= in_ctr; out.col(col) -= out_ctr; } // SVD Eigen::MatrixXd Cov = in * out.transpose(); Eigen::JacobiSVD svd(Cov, Eigen::ComputeThinU | Eigen::ComputeThinV); // Find the rotation double d = (svd.matrixV() * svd.matrixU().transpose()).determinant(); if (d > 0) d = 1.0; else d = -1.0; Eigen::Matrix3d I = Eigen::Matrix3d::Identity(3, 3); I(2, 2) = d; Eigen::Matrix3d R = svd.matrixV() * I * svd.matrixU().transpose(); // The final transform A.linear() = scale * R; A.translation() = scale*(out_ctr - R*in_ctr); return A; } iv::ndtorigin PoseToGPS(iv::ndtorigin xori,iv::ndttracepoint 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::ndtorigin 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; return xgpspos; } iv::ndttracepoint CalcPose(iv::ndtorigin xori, iv::gps::gpsimu xgpsimu) { double x_o,y_o,hdg_o; double x_c,y_c,hdg_c; GaussProjCal(xori.lon,xori.lat,&x_o,&y_o); GaussProjCal(xgpsimu.lon(),xgpsimu.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 - xgpsimu.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 diffang = atan(rel_y/rel_x) * 180.0/M_PI; double rel_hdg; rel_hdg = hdg_c - hdg_o; iv::ndttracepoint posex; posex.x = rel_x; posex.y = rel_y; posex.z = xgpsimu.height() - xori.height; posex.pitch = xgpsimu.pitch() - xori.pitch; posex.roll = xgpsimu.roll() - xori.roll; posex.yaw = rel_hdg; return posex; } MainWindow::MainWindow(QWidget *parent) : QMainWindow(parent), ui(new Ui::MainWindow) { ui->setupUi(this); // iv::ndtorigin xs; // xs.heading = 359.80 -0.72; //// xs.heading = 350; // xs.height = 0; // xs.pitch = 0; // xs.roll = 0; // xs.lat = 39.12051011; // xs.lon = 117.02730387; // iv::gps::gpsimu xgpsimuobj; // xgpsimuobj.set_lat(39.12059277); // xgpsimuobj.set_lon(117.02730218); // iv::ndttracepoint xt = CalcPose(xs,xgpsimuobj); mfCalPitch = 0.0; mfCalRoll = 0.0; mfCalYaw = 0.0; //创建参数组合框 CreateView(); m_nP= 0; //启动定时器 connect(&mTimer,SIGNAL(timeout()),this,SLOT(onTimer())); //启动mndtthread线程 mndtthread.start(); //设置窗口标题 setWindowTitle("ADC NDT Mapping ------ Lidar Localization"); mCalibMatrix<<1.0,0,0, 0,1.0,0, 0,0,1.0; Eigen::Matrix3d t = mCalibMatrix; tf::Matrix3x3 mat_l; // localizer mat_l.setValue(static_cast(t(0, 0)), static_cast(t(0, 1)), static_cast(t(0, 2)), static_cast(t(1, 0)), static_cast(t(1, 1)), static_cast(t(1, 2)), static_cast(t(2, 0)), static_cast(t(2, 1)), static_cast(t(2, 2))); double roll,pitch,yaw; mat_l.getRPY(roll, pitch, yaw, 1); std::ostrstream ostr; ostr<<"Rotation:"<plainTextEdit_Matrix->setPlainText(strres.substr(0,ostr.pcount()).data()); // ui->Dial->setStyleSheet("background-color:Black"); // ui->Dial->setStyleSheet("color:rgb(0,0,0)"); } MainWindow::~MainWindow() { mndtthread.requestInterruption(); while(!mndtthread.isFinished()) { } delete ui; } void MainWindow::on_pushButton_Load_clicked() { gsplit_range = mpLE_SplitRange->text().toDouble(); gsplit_filter = mpLE_FilterResolution->text().toDouble(); gndtorigin.lon = mpLE_OriginLon->text().toDouble(); gndtorigin.lat = mpLE_OriginLat->text().toDouble(); gndtorigin.height = mpLE_OriginHeight->text().toDouble(); gndtorigin.heading = mpLE_OriginHeading->text().toDouble(); gndtorigin.pitch = 0; gndtorigin.roll = 0; mnPac = 0; mbCalcCalib = false; mbCalcCalib_Roll = false; int ncalindex = mpCB_Mode->currentIndex(); if(ncalindex == 0) { mndtthread.setusecpu(); } else { if(ncalindex == 1) { mndtthread.setuseomp(); } else mndtthread.setusegpu(); } //获取文件路径 QString str = QFileDialog::getOpenFileName(this,tr("Open file"),"",tr("Record File(*.ivd)")); if(str.isEmpty())return; mvectorcalib.clear(); mvectorgps.clear(); mvectorrawgps.clear(); mvectortrace.clear(); mvectortracecalib.clear(); mvectorglobalgps.clear(); mbIVDSetOrigin = false; mndtthread.reset(); // ui->pushButton_Start->setEnabled(false); mbReplay = false; mMutex.lock(); if(mFile.isOpen())mFile.close(); mFile.setFileName(str); //以只读打开文件 if(mFile.open(QFile::ReadOnly)) { if(mbSaveDiff) { QFileInfo fileinfo = QFileInfo(str); std::string strpath = getenv("HOME"); strpath = strpath + "/map/"; strpath = strpath + fileinfo.fileName().toLatin1().data(); strpath = strpath +".txt"; if(mFileDiff.isOpen())mFileDiff.close(); mFileDiff.setFileName(strpath.data()); if(mFileDiff.open(QIODevice::ReadWrite)) { mbFileDiffOpen = true; char strout[1000]; snprintf(strout,1000,"Pos\tDiff\tndt_x\tndt_y\tndt_z\tndt_yaw\trtk_x\trtk_y\trtk_z\trtk_yaw\t\n"); mFileDiff.write(strout,strnlen(strout,1000)); mFileDiff.flush(); } else { mbFileDiffOpen = false; } } //设置当前文件打开状态 mbOpen = true; //文件大小 mnFileSize = mFile.size(); //读取到文件的位置 mnPos = 0; if(mnskip > 0) { int nReadSize = 0; int nDataSize; char * strData; char * strName; // 读取记录 bool bx = ReadARecord(nReadSize,&strName,&nDataSize,&strData); mnPac++; int nr= 1; while((nr < mnskip)&&bx) { delete strName; delete strData; bx = ReadARecord(nReadSize,&strName,&nDataSize,&strData); mnPac++; nr++; } delete strName; delete strData; } } // if(mbOpen) // { // ui->pushButton_Start->setEnabled(true); // } // else // { // ui->pushButton_Start->setEnabled(false); // } mbIVDSetOrigin = false; mbIVDSetGlobalOrigin = false; mMutex.unlock(); //更新地图参数 UpdateMapParam(); //启动定时器 mTimer.start(10); } //定时器执行程序 void MainWindow::onTimer() { int nP; std::string str; iv::ndttracepoint xndtpoint; qint64 xlidartime; //获取状态信息,获取全局变量g_np和gstr GetState(nP,str,xndtpoint,xlidartime); static double fOldDis = gsplit_cur; static double bFirst = true; //每执行一次point_ndtmapping操作,全局变量g_np会增加1,如果np有变化则更新文本框内的数据 if(nP != m_nP) { ui->plainTextEdit->setPlainText(str.c_str()); ui->lineEdit_Dis->setText(QString::number(gsplit_cur,'f',3)); mvectortrace.push_back(xndtpoint); mptraceview->SetNDTTrace(mvectortrace); mvectortracecalib.push_back(xndtpoint); calibcalc(&mvectortracecalib[mvectortracecalib.size()-1]); mptraceview->SetNDTCalibTrace(mvectortracecalib); bool bHaveRTKPoint = false; iv::ndttracepoint xtrrtk; while(mvectorrawgps.size()>0) { // std::cout<<" time diff: "<<(mvectorrawgps[0].recvtime - xlidartime)<=(xlidartime-30))) { iv::ndttracepoint xtr; xtr = mvectorrawgps[0].xtrace; mvectorgps.push_back(xtr); bHaveRTKPoint = true; xtrrtk = xtr; break; } mvectorrawgps.erase(mvectorrawgps.begin()); } if(mbFileDiffOpen) { if(((fabs(gsplit_cur - fOldDis )>=1.0) || bFirst)&&(bHaveRTKPoint)) { bFirst = false; fOldDis = gsplit_cur; char strout[1000]; double fdis = sqrt(pow(xndtpoint.x - xtrrtk.x,2)+pow(xndtpoint.y - xtrrtk.y,2)); snprintf(strout,1000,"%6.2f\t%6.2f\t%6.2f\t%6.2f\t%6.2f\t%6.2f\t%6.2f\t%6.2f\t%6.2f\t%6.2f\t\n", gsplit_cur,fdis,xndtpoint.x,xndtpoint.y,xndtpoint.z,xndtpoint.yaw, xtrrtk.x,xtrrtk.y,xtrrtk.z,xtrrtk.yaw); mFileDiff.write(strout,strnlen(strout,1000)); mFileDiff.flush(); } } mptraceview->SetGPSTrace(mvectorgps); mptraceview->SetGlobalGPSTrace(mvectorglobalgps); if(mvectorgps.size()>0) { iv::ndttracepoint xtr = mvectorgps[mvectorgps.size()-1]; char strtemp[1000]; snprintf(strtemp,1000,"%6.3f %6.3f %6.3f %6.3f", xtr.x,xtr.y,xtr.z,xtr.yaw); ui->lineEdit_GPS->setText(strtemp); } if(mvectortrace.size()>0) { iv::ndttracepoint xtr = mvectortrace[mvectortrace.size()-1]; char strtemp[1000]; snprintf(strtemp,1000,"%6.3f %6.3f %6.3f %6.3f", xtr.x,xtr.y,xtr.z,xtr.yaw); ui->lineEdit_NDT->setText(strtemp); } if(mvectortracecalib.size()>0) { iv::ndttracepoint xtr = mvectortracecalib[mvectortracecalib.size()-1]; char strtemp[1000]; snprintf(strtemp,1000,"%6.3f %6.3f %6.3f %6.3f", xtr.x,xtr.y,xtr.z,xtr.yaw); ui->lineEdit_Calib->setText(strtemp); } if((gsplit_cur>50.0)&&(mbCalcCalib == false)) { CalcCalib(); mbCalcCalib = true; } if((gsplit_cur>100.0) &&(mbCalcCalib_Roll == false)) { if(mvectortrace.size()>0) { if(fabs(mvectortrace[mvectortrace.size()-1].y) > 30.0) { CalcCalibRoll(); mbCalcCalib_Roll = true; } } } update(); } //记录执行了多少次 m_nP = nP; if(mbPause)return; bool bLidarGood = true; if(mndtthread.AbleNext())//如果可执行下一次 { int nReadSize = 0; int nDataSize; char * strData; char * strName; // 读取记录 bool bx = ReadARecord(nReadSize,&strName,&nDataSize,&strData); mnPac++; //循环读取数据,直到读取到"lidar_pc" while((bx == true)&&(strcmp(strName,gstrlidarmsg.data()) != 0)) { if((strcmp(strName,gstrimumsg.data()) == 0)) { iv::gps::gpsimu xgpsimu; bool bParse; bParse = xgpsimu.ParseFromArray(strData,nDataSize); if(bParse) { // if(fabs(xgpsimu.vd())>0.5) // { // qDebug("vd is %f",xgpsimu.vd()); // } // if(mbIVDSetOrigin == false) { if(xgpsimu.rtk_state() == 6) { double fheading = xgpsimu.heading(); fheading = fheading - 0; if(fheading < 0)fheading = fheading + 360; double flat,flon; mpLE_OriginLat->setText(QString::number(xgpsimu.lat(),'f',7)); mpLE_OriginLon->setText(QString::number(xgpsimu.lon(),'f',7)); mpLE_OriginHeight->setText(QString::number(xgpsimu.height(),'f',2)); mpLE_OriginHeading->setText(QString::number(fheading,'f',2)); gndtorigin.lon = mpLE_OriginLon->text().toDouble(); gndtorigin.lat = mpLE_OriginLat->text().toDouble(); gndtorigin.height = mpLE_OriginHeight->text().toDouble(); gndtorigin.heading = fheading; iv::ndttracepoint trarm; trarm.x = arm_x; trarm.y = arm_y; trarm.z = 0; trarm.yaw = 0; trarm.pitch = 0; trarm.roll = 0; gndtorigin = PoseToGPS(gndtorigin,trarm); mpLE_OriginLat->setText(QString::number(gndtorigin.lat,'f',7)); mpLE_OriginLon->setText(QString::number(gndtorigin.lon,'f',7)); mbIVDSetOrigin = true; if(mbIVDSetGlobalOrigin == false) { mbIVDSetGlobalOrigin = true; mglobalndtorigin = gndtorigin; } } } if(mbIVDSetOrigin) { iv::ndttracepoint xtr = CalcPose(gndtorigin,xgpsimu); 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); xtr.x = xtr.x + x - arm_x; xtr.y = xtr.y + y - arm_y; iv::gpstrace xg; if(xgpsimu.has_msgtime()) xg.recvtime = xgpsimu.msgtime(); else xg.recvtime = xgpsimu.time(); // xg.recvtime = xgpsimu.msgtime(); xg.xtrace = xtr; mvectorrawgps.push_back(xg); } if(mbIVDSetGlobalOrigin) { iv::ndttracepoint xtr = CalcPose(mglobalndtorigin,xgpsimu); 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); xtr.x = xtr.x + x - arm_x; xtr.y = xtr.y + y - arm_y; iv::gpstrace xg; mvectorglobalgps.push_back(xtr); } 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.has_acce_z()) // { // if(fabs(xgpsimu.acce_z() + 1.0)>0.5) // { // bLidarGood = true; // qDebug("acc z is %f vd is %f",xgpsimu.acce_z(),xgpsimu.vd()); // } // } } } // if(strcmp(strName,"ins550d_gpsimu") == 0) // { // iv::gps::gpsimu xgpsimu; // bool bParse; // bParse = xgpsimu.ParseFromArray(strData,nDataSize); // if(bParse) // { //// if(fabs(xgpsimu.vd())>0.5) //// { //// qDebug("vd is %f",xgpsimu.vd()); //// } // // // if(xgpsimu.has_acce_z()) // { // if(fabs(xgpsimu.acce_z() + 1.0)>0.5) // { // bLidarGood = true; // qDebug("acc z is %f vd is %f",xgpsimu.acce_z(),xgpsimu.vd()); // } // } // } // } delete strData; delete strName; // 读取一条记录并将读取的结果赋值给变量:strName,nDataSize,strData bx = ReadARecord(nReadSize,&strName,&nDataSize,&strData); mnPac++; } ui->lineEdit_Pac->setText(QString::number(mnPac)); //更新进度条 ui->horizontalSlider->setValue(mFile.pos()*100/mnFileSize); // 文件读取完毕,进度条设为100,停止计时器 if(bx == false) { ui->horizontalSlider->setValue(100); mTimer.stop(); } else//读取到lidar数据 { qDebug("find a lidar_pc"); //预处理数据 if(bLidarGood == true)procapcd(strData,nDataSize); else { bLidarGood = true; qDebug("skip 1 lidarpc"); } delete strData; delete strName; } } } void MainWindow::paintEvent(QPaintEvent *) { if(!mptraceview->IsHaveNew()) { return; } QImage image = mptraceview->GetImage(); mscene->clear(); mscene->addPixmap(QPixmap::fromImage(image)); mmyview->setScene(mscene); mmyview->show(); } //获取rh时间 inline QDateTime MainWindow::GetDateTimeFromRH(iv::RecordHead rh) { QDateTime dt; QDate datex; QTime timex; datex.setDate(rh.mYear,rh.mMon,rh.mDay); timex.setHMS(rh.mHour,rh.mMin,rh.mSec,rh.mMSec); dt.setDate(datex); dt.setTime(timex); return dt; } //处理数据 void MainWindow::procapcd(char *strdata, int nSize) { if(nSize <=16)return; //头文件大小 unsigned int * pHeadSize = (unsigned int *)strdata; if(*pHeadSize > nSize) { // std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<" data size is"<::Ptr point_cloud( new pcl::PointCloud()); // pcl::PointCloud::Ptr point_cloud( // new pcl::PointCloud()); // 获取名字 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; double yaw,pitch,roll; yaw = (mfCalYaw)*M_PI/180.0; pitch = (mfCalPitch)*M_PI/180.0; roll = (mfCalRoll)*M_PI/180.0; double x1,y1,z1; // 把点的数据赋值给点云对象中的变量 pcl::PointXYZI * p; p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize); for(i=0;iy; xp.y = p->x * (-1.0); xp.z = p->z; x1 = p->y; y1 = p->x * (-1.0); z1 = p->z; xp.x = x1*cos(yaw) -y1*sin(yaw); xp.y = x1*sin(yaw) + y1*cos(yaw); x1 = xp.x; z1 = xp.z; xp.z = z1*cos(pitch) -x1*sin(pitch); xp.x = z1*sin(pitch) + x1*cos(pitch); y1 = xp.y; z1 = xp.z; xp.y = y1*cos(roll) -z1*sin(roll); xp.z = y1*sin(roll) + z1*cos(roll); xp.intensity = p->intensity; point_cloud->push_back(xp); p++; } // 把点云数据传给处理点云数据的线程对象 mndtthread.procapcd(point_cloud); } inline bool MainWindow::ReadARecord(int & nRecSize,char ** pstrName, int * pnDataSize, char ** pstrData) { char strmark[10]; //消息总大小、消息头大小、消息名字大小、消息数据大小 int nTotalSize,nHeadSize,nNameSize,nDataSize; int nRead = mFile.read(strmark,1); if(nRead == 0)return false; //获取消息总大小、消息头大小、消息名字大小、消息数据大小 nRead = mFile.read((char *)&nTotalSize,sizeof(int)); if(nRead < sizeof(int))return false; nRead = mFile.read((char *)&nHeadSize,sizeof(int)); if(nRead < sizeof(int))return false; nRead = mFile.read((char *)&nNameSize,sizeof(int)); if(nReadtext().toDouble(); //存储地图 savemap(str.toLatin1().data(),filter_res); QString strtrace = str; strtrace = str.left(strtrace.size()-4); strtrace = strtrace + ".txt"; savetrace(strtrace.toLatin1().data()); QString strorigin = str; strorigin = str.left(strorigin.size()-4); strorigin = strorigin + ".ori"; saveorigin(strorigin.toLatin1().data()); } //创建左侧标题为"Param"的组合框 void MainWindow::CreateView() { // QDesktopWidget* desktopWidget = QApplication::desktop(); // QRect screenRect = desktopWidget->screenGeometry(); // g_nActScreenW = screenRect.width(); // g_nActScreenH = screenRect.height(); // qDebug("width = %d, height = %d",g_nActScreenW,g_nActScreenH); QGroupBox * gp1 = new QGroupBox(ui->centralWidget); gp1->setTitle(QStringLiteral("Param")); gp1->setGeometry(QRect(10,60,430,650)); QGridLayout * gll1 = new QGridLayout(ui->centralWidget); gp1->setLayout(gll1); 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(); mptraceview->start(); QLabel * pLabel = new QLabel(ui->centralWidget); pLabel->setGeometry(QRect(1400,60,80,30)); pLabel->setText("Scale View"); mpSlider = new QSlider(ui->centralWidget); mpSlider->setGeometry(QRect(1500,60,300,30)); mpSlider->setOrientation(Qt::Horizontal); mpSlider->setValue(1); mpSlider->setRange(1,10); connect(mpSlider,SIGNAL(valueChanged(int)),this,SLOT(onChangeViewFac(int))); pLabel = new QLabel(ui->centralWidget); pLabel->setGeometry(QRect(1400,150,90,30)); pLabel->setText("Cor Yaw:"); QLineEdit * pLE = new QLineEdit(ui->centralWidget); pLE->setGeometry(QRect(1500,150,90,30)); pLE->setText(QString::number(mfCalYaw)); mpLE_CalYaw = pLE; pLabel = new QLabel(ui->centralWidget); pLabel->setGeometry(QRect(1600,150,90,30)); pLabel->setText("Cor Pitch:"); pLE = new QLineEdit(ui->centralWidget); pLE->setGeometry(QRect(1700,150,90,30)); pLE->setText(QString::number(mfCalPitch)); mpLE_CalPitch = pLE; pLabel = new QLabel(ui->centralWidget); pLabel->setGeometry(QRect(1400,200,90,30)); pLabel->setText("Cor Roll:"); pLE = new QLineEdit(ui->centralWidget); pLE->setGeometry(QRect(1500,200,90,30)); pLE->setText(QString::number(mfCalRoll)); mpLE_CalRoll = pLE; QPushButton * pPB = new QPushButton(ui->centralWidget); pPB->setGeometry(QRect(1700,200,90,30)); pPB->setText("Set Cor"); connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onSetCor())); pLabel = new QLabel(ui->centralWidget); pLabel->setGeometry(QRect(1400,300,90,30)); pLabel->setText("Arm x:"); pLE = new QLineEdit(ui->centralWidget); pLE->setGeometry(QRect(1500,300,90,30)); pLE->setText(QString::number(mfCalYaw)); mpLE_Armx = pLE; pLabel = new QLabel(ui->centralWidget); pLabel->setGeometry(QRect(1600,300,90,30)); pLabel->setText("Arm y:"); pLE = new QLineEdit(ui->centralWidget); pLE->setGeometry(QRect(1700,300,90,30)); pLE->setText(QString::number(mfCalPitch)); mpLE_Army = pLE; pLabel = new QLabel(ui->centralWidget); pLabel->setGeometry(QRect(1400,350,260,30)); pLabel->setText("Arm is LIDAR TO GPS:"); pPB = new QPushButton(ui->centralWidget); pPB->setGeometry(QRect(1700,350,90,30)); pPB->setText("Set Arm"); connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onSetArm())); mmyview = new MyView(ui->centralWidget); mmyview->setObjectName(QStringLiteral("graphicsView")); mmyview->setGeometry(QRect(460, 430, 600, 600)); mmyview->setCacheMode(mmyview->CacheBackground); mscene = new QGraphicsScene; ui->pushButton_Reset->setEnabled(false); } void MainWindow::resizeEvent(QResizeEvent *event) { qDebug("resize"); QSize sizemain = ui->centralWidget->size(); qDebug("size x = %d y=%d",sizemain.width(),sizemain.height()); if((sizemain.width()>500) &&(sizemain.height()>450)) { mmyview->setGeometry(460,430,sizemain.width()-500,sizemain.height()-450); // ui->plainTextEdit->setGeometry(460,60,sizemain.width()-500,350); } } 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) { gl->setSpacing(10); int i = 0; QLabel * pl = new QLabel(this); pl->setText("Resolution"); pl->setFixedWidth(200); QLineEdit * ple = new QLineEdit(this); ple->setFixedWidth(100); ple->setText("1"); gl->addWidget(pl,i,0); gl->addWidget(ple,i,1); i++; mpLE_Resolution = ple; pl = new QLabel(this); pl->setText("Step Size"); pl->setFixedWidth(200); ple = new QLineEdit(this); ple->setFixedWidth(100); ple->setText("0.1"); gl->addWidget(pl,i,0); gl->addWidget(ple,i,1); i++; mpLE_StepSize = ple; pl = new QLabel(this); pl->setText("Transformation Epsilon"); pl->setFixedWidth(200); ple = new QLineEdit(this); ple->setFixedWidth(100); ple->setText("0.01"); gl->addWidget(pl,i,0); gl->addWidget(ple,i,1); i++; mpLE_TransFormationEpsilon = ple; pl = new QLabel(this); pl->setText("Maximum Iterations"); pl->setFixedWidth(200); ple = new QLineEdit(this); ple->setFixedWidth(100); ple->setText("30"); gl->addWidget(pl,i,0); gl->addWidget(ple,i,1); i++; mpLE_MaximumIterations = ple; pl = new QLabel(this); pl->setText("Leaf Size"); pl->setFixedWidth(200); ple = new QLineEdit(this); ple->setFixedWidth(100); ple->setText("2"); gl->addWidget(pl,i,0); gl->addWidget(ple,i,1); i++; mpLE_LeafSize = ple; pl = new QLabel(this); pl->setText("Minimum Scan Range"); pl->setFixedWidth(200); ple = new QLineEdit(this); ple->setFixedWidth(100); ple->setText("5"); gl->addWidget(pl,i,0); gl->addWidget(ple,i,1); i++; mpLE_MinimunScanRange = ple; pl = new QLabel(this); pl->setText("Maximum Scan Range"); pl->setFixedWidth(200); ple = new QLineEdit(this); ple->setFixedWidth(100); ple->setText("200"); gl->addWidget(pl,i,0); gl->addWidget(ple,i,1); i++; mpLE_MaximumScanRange = ple; pl = new QLabel(this); pl->setText("Minimum Add Scan Shift"); pl->setFixedWidth(200); ple = new QLineEdit(this); ple->setFixedWidth(100); ple->setText("1"); gl->addWidget(pl,i,0); gl->addWidget(ple,i,1); i++; mpLE_MinimumAddScanShfit = ple; pl = new QLabel(this); pl->setText("Mode"); pl->setFixedWidth(200); QComboBox * pcb = new QComboBox(this); pcb->addItem("CPU"); pcb->addItem("OpenMP"); #ifdef CUDA_FOUND pcb->addItem("GPU"); #endif pcb->setFixedWidth(100); gl->addWidget(pl,i,0); gl->addWidget(pcb,i,1); i++; mpCB_Mode = pcb; pl = new QLabel(this); pl->setText("Filter Resolution"); pl->setFixedWidth(200); ple = new QLineEdit(this); ple->setFixedWidth(100); ple->setText("0.2"); gl->addWidget(pl,i,0); gl->addWidget(ple,i,1); i++; mpLE_FilterResolution = ple; pl = new QLabel(this); pl->setText("Split Range"); pl->setFixedWidth(200); ple = new QLineEdit(this); ple->setFixedWidth(100); ple->setText("2000"); gl->addWidget(pl,i,0); gl->addWidget(ple,i,1); i++; mpLE_SplitRange = ple; pl = new QLabel(this); pl->setText("Origin Longitude"); pl->setFixedWidth(200); ple = new QLineEdit(this); ple->setFixedWidth(100); ple->setText("117"); gl->addWidget(pl,i,0); gl->addWidget(ple,i,1); i++; mpLE_OriginLon = ple; pl = new QLabel(this); pl->setText("Origin Latitude"); pl->setFixedWidth(200); ple = new QLineEdit(this); ple->setFixedWidth(100); ple->setText("39"); gl->addWidget(pl,i,0); gl->addWidget(ple,i,1); i++; mpLE_OriginLat =ple; pl = new QLabel(this); pl->setText("Origin Height"); pl->setFixedWidth(200); ple = new QLineEdit(this); ple->setFixedWidth(100); ple->setText("0"); gl->addWidget(pl,i,0); gl->addWidget(ple,i,1); i++; mpLE_OriginHeight = ple; pl = new QLabel(this); pl->setText("Origin Heading"); pl->setFixedWidth(200); ple = new QLineEdit(this); ple->setFixedWidth(100); ple->setText("90"); gl->addWidget(pl,i,0); gl->addWidget(ple,i,1); i++; mpLE_OriginHeading = ple; QPushButton * pPB = new QPushButton(this); pPB->setText("Change Param"); pPB->setFixedWidth(200); gl->addWidget(pPB,i,0); mpPB_ChangeParam = pPB; pPB = new QPushButton(this); pPB->setText("Split"); pPB->setFixedWidth(100); gl->addWidget(pPB,i,1); mpPB_Split = pPB; i++; connect(mpPB_ChangeParam,SIGNAL(clicked(bool)),this,SLOT(onChangeParam())); connect(mpPB_Split,SIGNAL(clicked(bool)),this,SLOT(onSplit())); QSpacerItem * xpsi2 = new QSpacerItem(200,1000,QSizePolicy::Maximum); gl->addItem(xpsi2,i,0); } void MainWindow::UpdateMapParam() { double resolution,stepsize,epsilon,maxiter,leafsize,minscan,maxscan,minscanshift; bool bUseGPU = false; resolution = mpLE_Resolution->text().toDouble(); stepsize = mpLE_StepSize->text().toDouble(); epsilon = mpLE_TransFormationEpsilon->text().toDouble(); maxiter = mpLE_MaximumIterations->text().toDouble(); leafsize = mpLE_LeafSize->text().toDouble(); minscan = mpLE_MinimunScanRange->text().toDouble(); maxscan = mpLE_MaximumScanRange->text().toDouble(); minscanshift = mpLE_MinimumAddScanShfit->text().toDouble(); SetParam(resolution,stepsize,epsilon,maxiter,leafsize,minscan,maxscan,minscanshift,bUseGPU); } void MainWindow::onChangeParam() { UpdateMapParam(); } void MainWindow::onSplit() { gbManualSave = true; } void MainWindow::on_pushButton_UsePoint_clicked() { if((mvectorgps.size() <1)||(mvectortrace.size()<1)) { return; } double x1,y1,z1,x2,y2,z2; iv::ndttracepoint xtrndt,xtrgps; xtrndt = mvectortrace[mvectortrace.size()-1]; xtrgps = mvectorgps[mvectorgps.size()-1]; iv::calib xcalib; xcalib.mndt = xtrndt; xcalib.mgps = xtrgps; mvectorcalib.push_back(xcalib); int i; char strout[10000]; snprintf(strout,10000,""); for(i=0;iplainTextEdit_NDTGPS->setPlainText(strout); } void MainWindow::on_pushButton_Calc_clicked() { int nsize = mvectorcalib.size(); Eigen::Matrix3Xd in(3, nsize), out(3, nsize); int a = in.cols(); a = a+1; for (int row = 0; row < in.rows(); row++) { // for (int col = 0; col < in.cols(); col++) { in(row, 0) = mvectorcalib[row].mndt.x; in(row, 1) = mvectorcalib[row].mndt.y; in(row, 2) = mvectorcalib[row].mndt.z; out(row,0) = mvectorcalib[row].mgps.x; out(row,1) = mvectorcalib[row].mgps.y; out(row,2) = mvectorcalib[row].mgps.z; // } } std::ostrstream ostr; Eigen::Affine3d A = Find3DAffineTransform(in, out); std::cout<(t(0, 0)), static_cast(t(0, 1)), static_cast(t(0, 2)), static_cast(t(1, 0)), static_cast(t(1, 1)), static_cast(t(1, 2)), static_cast(t(2, 0)), static_cast(t(2, 1)), static_cast(t(2, 2))); double roll,pitch,yaw; mat_l.getRPY(roll, pitch, yaw, 1); std::cout<<"roll:"<plainTextEdit_Matrix->setPlainText(strres.substr(0,ostr.pcount()).data()); mCalibMatrix = t; int i; for(i=0;iSetNDTCalibTrace(mvectortracecalib); } void MainWindow::on_pushButton_Clear_clicked() { mvectorcalib.erase(mvectorcalib.begin(),mvectorcalib.end()); ui->plainTextEdit_NDTGPS->setPlainText(""); } iv::ndttracepoint MainWindow::calibcalc(iv::ndttracepoint *p) { iv::ndttracepoint x; Eigen::Vector3d xin; xin(0) = p->x; xin(1) = p->y; xin(2) = p->z; Eigen::Vector3d xout; xout = mCalibMatrix * xin; p->x = xout(0); p->y = xout(1); p->z = xout(2); x = *p; x.x = p->x; x.y = p->y; x.z = p->z; return x; } void MainWindow::on_pushButton_Pause_clicked() { if(ui->pushButton_Pause->text() == "Pause") { ui->pushButton_Pause->setText("Conitinue"); ui->pushButton_Reset->setEnabled(true); mbPause = true; } else { ui->pushButton_Pause->setText("Pause"); ui->pushButton_Reset->setEnabled(false); mbPause = false; } } void MainWindow::on_pushButton_SetSkip_clicked() { mnskip = ui->lineEdit_skip->text().toInt(); } void MainWindow::on_pushButton_Reset_clicked() { mvectorcalib.clear(); mvectorgps.clear(); mvectorrawgps.clear(); mvectortrace.clear(); mvectortracecalib.clear(); mbIVDSetOrigin = false; mndtthread.reset(); } void MainWindow::onChangeViewFac(int nFac) { mptraceview->SetFac(nFac); } void MainWindow::onSetCor() { mfCalYaw = mpLE_CalYaw->text().toDouble(); mfCalPitch = mpLE_CalPitch->text().toDouble(); mfCalRoll = mpLE_CalRoll->text().toDouble(); } void MainWindow::onSetArm() { arm_x = mpLE_Armx->text().toDouble(); 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)); double calib_pitch = (-1.0) * asin((gps_z - ndt_z)/gps_dis); while(calib_pitch>2.0*M_PI) calib_pitch = calib_pitch - 2.0*M_PI; while(calib_pitch < 0)calib_pitch = calib_pitch + 2.0*M_PI; mpLE_CalcCalib_Pitch->setText(QString::number(calib_pitch * 180.0/M_PI)); } } } void MainWindow::CalcCalibRoll() { 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) &&(fabs(gps_y)>10)) { double calib_roll = (1.0) * asin((gps_z - ndt_z)/gps_y); while(calib_roll>2.0*M_PI) calib_roll = calib_roll - 2.0*M_PI; while(calib_roll < 0)calib_roll = calib_roll + 2.0*M_PI; mpLE_CalcCalib_Roll->setText(QString::number(calib_roll * 180.0/M_PI)); } } }