modularization/src/tool/adcndtmapping/mainwindow.cpp

#include "mainwindow.h"
#include "ui_mainwindow.h"

#include <ostream>
#include <strstream>

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    //创建参数组合框
    CreateView();

    m_nP= 0;
    //启动定时器
    connect(&mTimer,SIGNAL(timeout()),this,SLOT(onTimer()));
    //启动mndtthread线程
    mndtthread.start();
    //设置窗口标题
    setWindowTitle("ADC NDT Mapping ------ Lidar Localization");


}

MainWindow::~MainWindow()
{

    mndtthread.requestInterruption();
    while(!mndtthread.isFinished())
    {

    }
    delete ui;
}

void MainWindow::on_pushButton_Load_clicked()
{
    //获取文件路径
    QString str = QFileDialog::getOpenFileName(this,tr("Open file"),"",tr("Record File(*.ivd)"));
    if(str.isEmpty())return;


//    ui->pushButton_Start->setEnabled(false);
    mbReplay = false;
    mMutex.lock();



    if(mFile.isOpen())mFile.close();
    mFile.setFileName(str);
    //以只读打开文件
    if(mFile.open(QFile::ReadOnly))
    {
        //设置当前文件打开状态
        mbOpen = true;
        //文件大小
        mnFileSize = mFile.size();
        //读取到文件的位置
        mnPos = 0;
    }
//    if(mbOpen)
//    {
//        ui->pushButton_Start->setEnabled(true);
//    }
//    else
//    {
//        ui->pushButton_Start->setEnabled(false);
//    }
    mMutex.unlock();
    //更新地图参数
    UpdateMapParam();
    //启动定时器
    mTimer.start(10);
}
//定时器执行程序
void MainWindow::onTimer()
{
    int nP;
    std::string str;
    //获取状态信息，获取全局变量g_np和gstr
    GetState(nP,str);
    //每执行一次point_ndtmapping操作，全局变量g_np会增加1，如果np有变化则更新文本框内的数据
    if(nP != m_nP)
    {
        ui->plainTextEdit->setPlainText(str.c_str());
    }
    //记录执行了多少次
    m_nP = nP;

    if(mndtthread.AbleNext())//如果可执行下一次
    {

        int nReadSize = 0;
        int nDataSize;
        char * strData;
        char * strName;
//        读取记录
        bool bx = ReadARecord(nReadSize,&strName,&nDataSize,&strData);
        //循环读取数据，直到读取到"lidar_pc"
        while((bx == true)&&(strcmp(strName,"lidar_pc") != 0))
        {

            delete strData;
            delete strName;
//            读取一条记录并将读取的结果赋值给变量：strName,nDataSize,strData
            bx = ReadARecord(nReadSize,&strName,&nDataSize,&strData);
        }
        //更新进度条
        ui->horizontalSlider->setValue(mFile.pos()*100/mnFileSize);
//        文件读取完毕，进度条设为100，停止计时器
        if(bx == false)
        {
            ui->horizontalSlider->setValue(100);
            mTimer.stop();
        }
        else//读取到lidar数据
        {
            qDebug("find a lidar_pc");
            //预处理数据
            procapcd(strData,nDataSize);
            delete strData;
            delete strName;
        }


    }
}
//获取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"<<nSize<<std::endl;
    }
    //构建点云对象
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());

//    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::PointXYZI xp;
        xp.x = p->y;
        xp.y = p->x * (-1.0);
        xp.z = p->z;
        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(nRead<sizeof(int))return false;
    nRead = mFile.read((char *)&nDataSize,sizeof(int));
    if(nRead<sizeof(int))return false;
    //校验数据关系
    if(nTotalSize !=(nHeadSize + nNameSize + nDataSize + 4*sizeof(int) ))
    {
        return false;
    }

    iv::RecordHead rh;
    char * strName = new char[1000];
    char * strData = new char[nDataSize];
    //获取时间
    nRead = mFile.read((char *)&rh,sizeof(iv::RecordHead));
    if(nRead < sizeof(iv::RecordHead))
    {
        delete strData;
        delete strName;
        return false;
    }

    mdtcurpos = GetDateTimeFromRH(rh);
    //获取名字
    nRead = mFile.read(strName,nNameSize);
    if(nRead < nNameSize)
    {
        delete strData;
        delete strName;
        return false;
    }
//    使字符数组变为字符串
    strName[nNameSize] = 0;
//    qDebug(strName);
//    qDebug("file pos is %d ms is %d",mFile.pos(),rh.mMSec);
    //获取雷达数据
    nRead = mFile.read(strData,nDataSize);
    if(nRead < nDataSize)
    {
        delete strData;
        delete strName;
        return false;
    }

    //Share Data
    //指针赋值
    *pnDataSize = nDataSize;
    *pstrName = strName;
    *pstrData = strData;


//    delete strData;
    nRecSize = nTotalSize + 1;
    return true;

}
//点击save按钮
void MainWindow::on_pushButton_Save_clicked()
{
//    获取存储的路径
    QString str = QFileDialog::getSaveFileName(this,tr("Open file"),"",tr("PCD File(*.pcd)"));
    if(str.isEmpty())return;

    if(!str.contains(".pcd"))str = str + ".pcd";
    //点云过滤网格尺寸
    double filter_res = mpLE_FilterResolution->text().toDouble();
    //存储地图
    savemap(str.toLatin1().data(),filter_res);
}
//创建左侧标题为"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,350,400));
    QGridLayout * gll1 = new QGridLayout(ui->centralWidget);
    gp1->setLayout(gll1);

    CreateParamView(gll1);
}
//创建组合框内的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("GPU");
    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;

    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);
}