modularization/src/tool/view_ndtmatching/mainwindow.cpp

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

#include <pcl/io/ply_io.h>
#include <pcl/io/obj_io.h>

#include <QMessageBox>

static pose gCurPose;

static double gView_Z = 100.0;

static bool gbAutoCam = true;

static int gnViewMode = 0;

static double gfViewAngle = 30.0;

#include <cmath>

struct Quaternion {
    double w, x, y, z;
};

struct EulerAngles {
    double roll, pitch, yaw;
};

EulerAngles ToEulerAngles(Quaternion q) {
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (std::abs(sinp) >= 1)
        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles.pitch = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}


Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
    // Abbreviations for the various angular functions
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);

    Quaternion q;
    q.w = cy * cp * cr + sy * sp * sr;
    q.x = cy * cp * sr - sy * sp * cr;
    q.y = sy * cp * sr + cy * sp * cr;
    q.z = sy * cp * cr - cy * sp * sr;

    return q;
}

double mpos_x = 0;

void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
{
    //设置背景颜色
    viewer.setBackgroundColor(0.0,0.0,0.0);
   viewer.resetCamera();

//   pcl::PointXYZ mass_center(0,0,0);
//   pcl::PointXYZ kinectZ(0,0,-100);

//    viewer.addLine(mass_center,kinectZ,1.0f,1.0f,1.0f,"KinectZ");

//  pcl::PointXYZ kinectZ2(0,0,100);
//   viewer.addLine(mass_center,kinectZ2,1.0f,1.0f,1.0f,"KinectZ2");

   viewer.addCube(-0.9,0.9,-2.3,2.3,-1.9,-0.4,0.0,0.0,1.0,"car",0);


}

void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
{
//    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" psycho."<<std::endl;

    viewer.removeShape("car");

    Eigen::Vector3f trans;

    Quaternion q = ToQuaternion(gCurPose.yaw,gCurPose.pitch,gCurPose.roll);
    Eigen::Quaternionf quat(q.w,q.x,q.y,q.z);
    trans(0) = gCurPose.x;trans(1) = gCurPose.y;trans(2) = gCurPose.z;

 //   viewer.addCube(mpos_x+20 -0.9,mpos_x + 20+0.9,-2.3,2.3,-1.9,-0.4,0.0,0.0,1.0,"car",0);
    viewer.addCube(trans,quat,6.0,3.0,1.0,"car");

    double yaw_calc = M_PI/2.0 - gCurPose.yaw;

    if(gbAutoCam)
    {
        if(gnViewMode == 0)
            viewer.setCameraPosition(gCurPose.x,gCurPose.y,gView_Z,gCurPose.x,gCurPose.y,gCurPose.z,sin(yaw_calc),cos(yaw_calc),0);

        if(gnViewMode == 1)
        {
            double gfZsin = gView_Z * sin(gfViewAngle * M_PI/180.0);
            double gfZcos = gView_Z * cos(gfViewAngle * M_PI/180.0);

            double fcam_x = gCurPose.x + gfZcos * cos(gCurPose.yaw+M_PI);
            double fcam_y = gCurPose.y + gfZcos * sin(gCurPose.yaw+M_PI);
            viewer.setCameraPosition(fcam_x,fcam_y,gfZsin,gCurPose.x,gCurPose.y,gCurPose.z,sin(yaw_calc),cos(yaw_calc),0);
        }
    }

    std::stringstream ss;
 //   viewer.setCameraPosition(mpos_x,0,30,mpos_x + 20,0,0,0,0,0);
 //   ss << "Once per viewer loop: " << count++;

}





MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    mpLabelStatus = new QLabel("No NDT Pos",ui->statusbar);
    mpLabelStatus->setMinimumWidth(350);

//    ui->pushButton_EnableRelocation->setEnabled(false);

    ui->pushButton_Test->setVisible(false);

    ui->horizontalSlider->setRange(5,300);
    ui->horizontalSlider->setValue(100);

    ui->comboBox->addItem("Look Down");
    ui->comboBox->addItem("Tracking");

    ui->checkBox->setChecked(true);

    mCurPose.x = 0; mCurPose.y = 0; mCurPose.z = 0;
    mCurPose.yaw = 0; mCurPose.pitch = 0; mCurPose.roll = 0;

    gCurPose = mCurPose;


\
    mpthreadpcd = new std::thread(&MainWindow::threadpcdview,this);


    ModuleFun fun1 =std::bind(&MainWindow::UpdateNDTPos,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpandtpos = iv::modulecomm::RegisterRecvPlus("ndtpos",fun1);

    ModuleFun fun2 =std::bind(&MainWindow::UpdatePCDMap,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpapcdmap = iv::modulecomm::RegisterRecvPlus("pcdmap",fun2);

    mparelocate = iv::modulecomm::RegisterSend("relocate",10000,1);

    connect(this,SIGNAL(ndtposupdate()),this,SLOT(onndtposupdate()));

    setWindowTitle("NDT Matching View & Relocation Enable");

}

MainWindow::~MainWindow()
{
    mbRun = false;
    mpthreadpcd->join();
    iv::modulecomm::Unregister(mpandtpos);
    iv::modulecomm::Unregister(mparelocate);
    iv::modulecomm::Unregister(mpapcdmap);
    delete ui;
}


void MainWindow::threadpcdview()
{
     mpviewer = new pcl::visualization::CloudViewer("Cloud View");

     pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                 new pcl::PointCloud<pcl::PointXYZI>());

     pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud2(
                 new pcl::PointCloud<pcl::PointXYZI>());

     char strpcpath[256];
     snprintf(strpcpath,256,"%s/map/map.pcd",getenv("HOME"));

     pcl::io::loadPCDFile<pcl::PointXYZI>(strpcpath,*point_cloud);

 //    pcl::io::loadOBJFile("/home/yuchuli/car.obj",*point_cloud);
//     pcl::io::loadPLYFile("/home/yuchuli/car.ply",*point_cloud);
     mpviewer->showCloud(point_cloud);




     //This will only get called once
     mpviewer->runOnVisualizationThreadOnce (viewerOneOff);



     mpviewer->runOnVisualizationThread (viewerPsycho);


     while((!mpviewer->wasStopped()) && mbRun)
     {

         if(mbpcdmapupdate)
         {
             std::string strpcdmap;
             mmutexpcdmap.lock();
             strpcdmap = mstrpcdmappath;
             mbpcdmapupdate = false;
             mmutexpcdmap.unlock();

             pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud2(
                         new pcl::PointCloud<pcl::PointXYZI>());

              pcl::io::loadPCDFile<pcl::PointXYZI>(strpcdmap,*point_cloud2);
              mpviewer->showCloud(point_cloud2);
              std::cout<<" use new pcd map. path: "<<strpcdmap.data()<<std::endl;
         }

         std::this_thread::sleep_for(std::chrono::milliseconds(10));

     }

     delete mpviewer;

}


void MainWindow::on_pushButton_Test_clicked()
{
    double x = ui->lineEdit_x->text().toDouble();
    double y = ui->lineEdit_y->text().toDouble();
    double z = ui->lineEdit_z->text().toDouble();
    double yaw = ui->lineEdit_yaw->text().toDouble();
    double pitch = ui->lineEdit_pitch->text().toDouble();
    double roll = ui->lineEdit_roll->text().toDouble();
    mCurPose.x = x;
    mCurPose.y = y;
    mCurPose.z = z;
    mCurPose.yaw = yaw;
    mCurPose.pitch = pitch;
    mCurPose.roll = roll;

    gCurPose = mCurPose;
}

void MainWindow::on_pushButton_EnableRelocation_clicked()
{
    static int64_t nLastSend = 0;

    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;

    if(abs(nnow - nLastSend) < 180000)
    {
        QMessageBox::warning(this,tr("Warning"),tr("EnableRelocation Must Interval 3 minutes. "),QMessageBox::YesAll);
        return;
    }

    if(mfTransProb >= 2.0)
    {

        QMessageBox::StandardButton button;
        button=QMessageBox::question(this,tr("Relocate"),QString(tr("TransProblity is more than 2.0,do you want relocation?")),QMessageBox::Yes|QMessageBox::No);
        if(button==QMessageBox::No)
        {
            return;
        }
        else if(button==QMessageBox::Yes)
        {
        }

    }

    iv::ndt::relocate xreloc;
    xreloc.set_benable(true);
    int nbytesize = xreloc.ByteSize();
    std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
    if(xreloc.SerializeToArray(pstr_ptr.get(),nbytesize))
    {
        iv::modulecomm::ModuleSendMsg(mparelocate,pstr_ptr.get(),nbytesize);
    }
    else
    {
        QMessageBox::warning(this,tr("Warning"),tr("Relocate Message Serialize Fail."),QMessageBox::YesAll);
    }
    nLastSend = nnow;
}

void MainWindow::on_horizontalSlider_valueChanged(int value)
{
    gView_Z = value;
}

void MainWindow::on_checkBox_clicked()
{
    if(ui->checkBox->isChecked())
    {
       gbAutoCam = true;
    }
    else
    {
        gbAutoCam = false;
    }
}

void MainWindow::on_comboBox_currentIndexChanged(int index)
{
    gnViewMode = index;
}

void MainWindow::UpdateNDTPos(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;

    iv::lidar::ndtpos xndtpos;
    if(xndtpos.ParseFromArray(strdata,nSize))
    {
        mmutexndtpos.lock();
        mndtpos.CopyFrom(xndtpos);
        mbndtposupdate = true;
        mmutexndtpos.unlock();
        emit ndtposupdate();
    }
    else
    {
        std::cout<<"MainWindow::UpdateNDTPos Parse NDTPos Fail."<<std::endl;
    }

}

void MainWindow::onndtposupdate()
{
    if(mbndtposupdate)
    {
        iv::lidar::ndtpos xndtpos;
        mmutexndtpos.lock();
        xndtpos.CopyFrom(mndtpos);
        mbndtposupdate = false;
        mmutexndtpos.unlock();
        gCurPose.x = xndtpos.pose_x();
        gCurPose.y = xndtpos.pose_y();
        gCurPose.z = xndtpos.pose_z();
        gCurPose.yaw = xndtpos.pose_yaw();
        gCurPose.pitch = xndtpos.pose_pitch();
        gCurPose.roll = xndtpos.pose_roll();
        ui->lineEdit_prob->setText(QString::number(xndtpos.trans_probability(),'f',3));
        ui->lineEdit_score->setText(QString::number(xndtpos.fitness_score(),'f',3));
        ui->lineEdit_x->setText(QString::number(gCurPose.x,'f',3));
        ui->lineEdit_y->setText(QString::number(gCurPose.y,'f',3));
        ui->lineEdit_z->setText(QString::number(gCurPose.z,'f',3));
        ui->lineEdit_yaw->setText(QString::number(gCurPose.yaw,'f',3));
        ui->lineEdit_pitch->setText(QString::number(gCurPose.pitch,'f',3));
        ui->lineEdit_roll->setText(QString::number(gCurPose.roll,'f',3));

        mfTransProb = xndtpos.trans_probability();

        mpLabelStatus->setText("NDT Update Time:" + QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss:zzz"));
    }
}

void MainWindow::UpdatePCDMap(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    (void)index;
    (void)dt;
    (void)strmemname;
    iv::ndt::pcdmap xpcdmap;
    std::cout<<" update ndt map "<<std::endl;
    if(xpcdmap.ParseFromArray(strdata,nSize))
    {
        mmutexpcdmap.lock();
        mstrpcdmappath = xpcdmap.strpath();
        mbpcdmapupdate = true;
        mmutexpcdmap.unlock();

    }
    else
    {
        std::cout<<"UpdatePCDMap Fail."<<std::endl;
    }
}