123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606 |
- #include "mainwindow.h"
- #include "ui_mainwindow.h"
- #include <iostream>
- #include <QFileDialog>
- #include <QTimer>
- #include <functional>
- MainWindow::MainWindow(QWidget *parent)
- : QMainWindow(parent)
- , ui(new Ui::MainWindow)
- {
- ui->setupUi(this);
- ui->actionNoClassification->setChecked(true);
- ui->actionNoise->setChecked(true);
- ui->actionGround->setChecked(true);
- ui->actionTraversableUnder->setChecked(true);
- ui->actionObstacle->setChecked(true);
- ui->actionInvalid->setChecked(true);
- ui->actionCar->setChecked(true);
- ui->actionTruck->setChecked(true);
- ui->actionMotorcycle->setChecked(true);
- ui->actionBicycle->setChecked(true);
- ui->actionPedestrian->setChecked(true);
- ui->actionAnimal->setChecked(true);
- ui->actionHazard->setChecked(true);
- ui->actionUnknown->setChecked(true);
- vtkSmartPointer<vtkRenderer> renderer = vtkSmartPointer<vtkRenderer>::New();
- vtkSmartPointer<vtkGenericOpenGLRenderWindow> renderWindow = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
- renderWindow->AddRenderer(renderer);
- view.reset(new pcl::visualization::PCLVisualizer(renderer,renderWindow,"viewer",false));
- #if VTK890
- view->setupInteractor(ui->vtk->interactor(),ui->vtk->renderWindow());
- #else
- view->setupInteractor(ui->vtk->GetInteractor(),ui->vtk->GetRenderWindow());
- #endif
- #if VTK890
- this->ui->vtk->setRenderWindow(renderWindow);
- #else
- this->ui->vtk->SetRenderWindow(renderWindow);
- #endif
- DrawAxis();
- // Create a sphere.
- int i;
- for(i=0;i<NUM_DET_MAX;i++){
- detSource[i]->SetBounds(-0.001,-0.001,-0.001,0.001,0.001,0.001);
- detSource[i]->Update();
- detmapper[i]->SetInputData(detSource[i]->GetOutput());
- // Create an actor.
- vtkNew<vtkActor> actor;
- vtkNew<vtkNamedColors> colors;
- actor->GetProperty()->SetColor(colors->GetColor3d("Yellow").GetData());
- actor->SetMapper(detmapper[i]);
- renderer->AddActor(actor);
- }
- for(i=0;i<NUM_OBJ_MAX;i++){
- sphereSource[i]->SetCenter(0,0,0);
- sphereSource[i]->SetRadius(0.001);
- sphereSource[i]->Update();
- mapper[i]->SetInputData(sphereSource[i]->GetOutput());
- // Create an actor.
- vtkNew<vtkActor> actor;
- vtkNew<vtkNamedColors> colors;
- actor->GetProperty()->SetColor(colors->GetColor3d("Red").GetData());
- actor->SetMapper(mapper[i]);
- renderer->AddActor(actor);
- }
- QTimer * timer1 = new QTimer();
- connect(timer1,SIGNAL(timeout()),this,SLOT(onTimer()));
- // timer1->start(100);
- connect(this,SIGNAL(detupdate()),this,SLOT(onDetUpdate()));
- #ifdef USE_MODULECOMM
- ModuleFun fundetect = std::bind(&MainWindow::UpdateDet,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
- mpadet = iv::modulecomm::RegisterRecvPlus("radar4ddet",fundetect);
- ModuleFun funojbect = std::bind(&MainWindow::UpdateObj,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
- mpaobj = iv::modulecomm::RegisterRecvPlus("radar4dobj",funojbect);
- #else
- mprecv = new ars548recv();
- mpthreaddet = new std::thread(&MainWindow::threaddet,this);
- mpthreadobj = new std::thread(&MainWindow::threadobj,this);
- #endif
- setWindowTitle("radar4d 3d Viewer");
- }
- MainWindow::~MainWindow()
- {
- mbThreadRun = false;
- #ifdef USE_MODULECOMM
- iv::modulecomm::Unregister(mpaobj);
- iv::modulecomm::Unregister(mpadet);
- #else
- mpthreadobj->join();
- mpthreaddet->join();
- delete mprecv;
- #endif
- delete ui;
- }
- void MainWindow::DrawAxis()
- {
- bool bGrid = false;
- pcl::PointXYZ p1(-300,0,0);
- pcl::PointXYZ p2(300,0,0);
- view->addLine(p1,p2,1.0f,1.0f,1.0f,"axis_x");
- p1.x = 0;p1.y = -300;
- p2.x= 0;p2.y = 300;
- view->addLine(p1,p2,1.0f,1.0f,1.0f,"axis_y");
- int i;
- if(bGrid)
- {
- for(i=-30;i<=30;i++)
- {
- if(i==0)continue;
- p1.x = -300;p1.y = i*10;
- p2.x = 300; p2.y = i*10;
- char strname[256];
- snprintf(strname,256,"axisgx_%d",i);
- view->addLine(p1,p2,1.0f,1.0f,1.0f,strname);
- p1.x = i*10;p1.y = -300;
- p2.x = i*10; p2.y = 300;
- snprintf(strname,256,"axisgy_%d",i);
- view->addLine(p1,p2,1.0f,1.0f,1.0f,strname);
- }
- }
- else
- {
- for(i=-30;i<=30;i++)
- {
- if(i==0)continue;
- p1.x = 0;p1.y = i*10;
- p2.x = 1; p2.y = i*10;
- char strname[256];
- snprintf(strname,256,"axisgx_%d",i);
- view->addLine(p1,p2,1.0f,1.0f,1.0f,strname);
- p1.x = i*10;p1.y = 0;
- p2.x = i*10; p2.y = 1;
- snprintf(strname,256,"axisgy_%d",i);
- view->addLine(p1,p2,1.0f,1.0f,1.0f,strname);
- }
- }
- for(i=-6;i<=6;i++)
- {
- char strname[256];
- char strtext[256];
- double orientation[3];
- orientation[0] = 1.0;orientation[1] = 0.0;orientation[2] = 0.0;
- p1.x = i*50;p1.y = 1;
- snprintf(strname,256,"textx_%d",i);
- snprintf(strtext,256,"%d",i*50);
- if(i%2 == 0)
- view->addText3D(strtext,p1,orientation,1.0,1.0,1.0,1.0,strname);
- else
- view->addText3D(strtext,p1,orientation,0.5,1.0,1.0,1.0,strname);
- if(i== 0)continue;
- orientation[0] = 1.0;orientation[1] =0.0;orientation[2] = 0.0;
- p1.x = 1;p1.y = i*50+1;
- snprintf(strname,256,"texty_%d",i);
- snprintf(strtext,256,"%d",i*50);
- if(i%2 == 0)
- view->addText3D(strtext,p1,orientation,1.0,1.0,1.0,1.0,strname);
- else
- view->addText3D(strtext,p1,orientation,0.5,1.0,1.0,1.0,strname);
- }
- // double base_size = 0.2;
- // double base_height = 0.2;
- // double sigx = 15;
- // double sigy = 15;
- // double sigz = 1.0;
- // view->addCube(sigx-base_size/2.0,sigx + base_size/2.0,sigy-base_size/2.0,sigy+base_size/2.0,sigz-base_height/2.0,sigz + base_height/2.0,1.0,0.0,0.0,"cube1");
- view->setCameraPosition(0,0,100,0,0,0,0,1,0);
- view->resetCamera(); //视角
- // view->addText("300",300,5,10,1.0f,0.0f,0.0f,"txt1");
- }
- void MainWindow::resizeEvent(QResizeEvent *event)
- {
- (void)event;
- QSize sizemain = ui->centralwidget->size();
- ui->vtk->setGeometry(0,0,sizemain.width()-0,sizemain.height()-0);
- }
- void MainWindow::onTimer()
- {
- int64_t nstart = std::chrono::system_clock::now().time_since_epoch().count();
- static bool bSetCam = false;
- if(bSetCam == false)
- {
- bSetCam = true;
- view->setCameraPosition(0,30,130,0,30,0,0,1,0);
- // view->resetCamera(); //视角
- }
- static int nindex = 0;
- nindex++;
- if(nindex>1000)nindex = 0;
- double base_size = 0.5;
- double base_height = 0.5;
- double sigx = 15;
- double sigy = 15 + nindex * 0.01;
- double sigz = 1.0;
- (void)base_size;
- (void)base_height;
- (void)sigx;
- (void)sigz;
- // view->removeShape("cube1");
- // view->addCube(sigx-base_size/2.0,sigx + base_size/2.0,sigy-base_size/2.0,sigy+base_size/2.0,sigz-base_height/2.0,sigz + base_height/2.0,1.0,0.0,0.0,"cube1");
- int i;
- for(i=0;i<100;i++){
- sphereSource[i]->SetCenter(i, sigy, 0.0);
- sphereSource[i]->Update();
- }
- // Create a mapper.
- // mapper->SetInputData(sphereSource->GetOutput());
- #if VTK890
- ui->vtk->renderWindow()->Render();
- #else
- ui->vtk->GetRenderWindow()->Render();
- #endif
- int64_t nend = std::chrono::system_clock::now().time_since_epoch().count();
- std::cout<<" update use : "<<(nend -nstart)/1e6<<std::endl;
- }
- void MainWindow::onDetUpdate()
- {
- static int64_t nlastup = 0;
- int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
- int64_t ndiff = (nnow - nlastup)/1e6;
- if(abs(ndiff)<10)
- {
- // nlastup = nnow;
- return;
- }
- bool bdetupdate = false;
- bool bobjupdate = false;
- int64_t nstart = std::chrono::system_clock::now().time_since_epoch().count();
- iv::radar::radar4ddetectarray xdetarray;
- mmutexdet.lock();
- if(mbdetupdate){
- xdetarray.CopyFrom(mdetarray);
- mbdetupdate = false;
- bdetupdate = true;
- }
- mmutexdet.unlock();
- iv::radar::radar4dobjectarray xobjarray;
- mmutexobj.lock();
- if(mbobjupdate){
- xobjarray.CopyFrom(mobjarray);
- mbobjupdate = false;
- bobjupdate = true;
- }
- mmutexobj.unlock();
- // static int nskip = 0;
- // nskip++;
- // if(nskip <2)
- // {
- // return;
- // }
- // nskip = 0;
- static bool bSetCam = false;
- if(bSetCam == false)
- {
- bSetCam = true;
- view->setCameraPosition(0,30,130,0,30,0,0,1,0);
- // view->resetCamera(); //视角
- }
- int i;
- double base_size = 0.2;
- double obj_size = 0.5;
- double sigx = 15;
- double sigy = 15 ;
- double sigz = 1.0;
- int nsize;
- if(bdetupdate){
- nsize = static_cast<int>(xdetarray.mdet_size());
- if(nsize > NUM_DET_MAX){
- nsize = NUM_DET_MAX;
- }
- for(i=0;i<nsize;i++)
- {
- iv::radar::radar4ddetect * pdet = xdetarray.mutable_mdet(i);
- if((pdet->detection_radial_distance()<0.001) || (pdet->detection_classification() != iv::radar::radar4ddetect::Obstacle))
- {
- detSource[i]->SetBounds(1000.0,1000.1,-0.001,0.001,0.001,0.001);
- detSource[i]->Update();
- continue;
- }
- bool bshow = false;
- if(ui->actionAll->isChecked())bshow = true;
- if((ui->actionNoClassification->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::NoClassification))bshow = true;
- if((ui->actionNoise->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::Noise))bshow = true;
- if((ui->actionGround->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::Ground))bshow = true;
- if((ui->actionTraversableUnder->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::TraversableUnder))bshow = true;
- if((ui->actionObstacle->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::Obstacle))bshow = true;
- if((ui->actionInvalid->isChecked())&&(pdet->detection_classification() == iv::radar::radar4ddetect::Invalid))bshow = true;
- if(bshow == true){
- sigx = pdet->detection_radial_distance() * cos(pdet->unaligned_detection_elevation_angle()) * cos(pdet->unaligned_detection_azimuth_angle() + M_PI/2.0);
- sigy = pdet->detection_radial_distance() * cos(pdet->unaligned_detection_elevation_angle()) * sin(pdet->unaligned_detection_azimuth_angle() + M_PI/2.0);
- sigz = pdet->detection_radial_distance() * sin(pdet->unaligned_detection_elevation_angle());
- detSource[i]->SetBounds(sigx-base_size/2.0,sigx+base_size/2.0,sigy - base_size/2.0,sigy + base_size/2.0, sigz - base_size/2.0,
- sigz + base_size/2.0);
- detSource[i]->Update();
- }
- else {
- detSource[i]->SetBounds(1000.0,1000.1,-0.001,0.001,0.001,0.001);
- detSource[i]->Update();
- }
- }
- }
- if(bobjupdate){
- nsize = static_cast<int>(xobjarray.mobj_size());
- if(nsize > NUM_OBJ_MAX){
- nsize = NUM_OBJ_MAX;
- }
- for(i=0;i<nsize;i++)
- {
- iv::radar::radar4dobject * pobj = xobjarray.mutable_mobj(i);
- // std::cout<<" i: "<<i<<" unkown: "<<pobj->u_classification_unknown()<<" car: "<<pobj->u_classification_car()
- // <<" truck: "<<pobj->u_classification_truck()
- // <<" motocycle: "<<pobj->u_classification_motorcycle()
- // <<" bycycle: "<<pobj->u_classification_bicycle()
- // <<" ped: "<<pobj->u_classification_pedestrian()
- // <<" animal: "<<pobj->u_classification_animal()
- // <<" hazard: "<<pobj->u_classification_hazard()
- // <<"exist: "<<pobj->u_existence_probability()<<std::endl;
- sigx = pobj->u_position_y() * (-1);
- sigy = pobj->u_position_x();
- sigz = pobj->u_position_z();
- bool bShow = false;
- unsigned int nMinProb = 30;
- if(ui->actionObjectAll->isChecked())bShow = true;
- if((ui->actionCar->isChecked())&&(pobj->u_classification_car()>=nMinProb))bShow = true;
- if((ui->actionTruck->isChecked())&&(pobj->u_classification_truck()>=nMinProb))bShow = true;
- if((ui->actionMotorcycle->isChecked())&&(pobj->u_classification_motorcycle()>=nMinProb))bShow = true;
- if((ui->actionBicycle->isChecked())&&(pobj->u_classification_bicycle()>=nMinProb))bShow = true;
- if((ui->actionPedestrian->isChecked())&&(pobj->u_classification_pedestrian()>=nMinProb))bShow = true;
- if((ui->actionAnimal->isChecked())&&(pobj->u_classification_animal()>=nMinProb))bShow = true;
- if((ui->actionHazard->isChecked())&&(pobj->u_classification_hazard()>=nMinProb))bShow = true;
- if((ui->actionUnknown->isChecked())&&(pobj->u_classification_unknown()>=nMinProb))bShow = true;
- if((pobj->u_existence_probability() >0)&&bShow){
- sphereSource[i]->SetCenter(sigx,sigy,sigz);
- sphereSource[i]->SetRadius(obj_size/2.0);
- }
- else{
- sphereSource[i]->SetCenter(0.0,0.0,0.0);
- sphereSource[i]->SetRadius(0.001);
- }
- sphereSource[i]->Update();
- }
- }
- // std::cout<<"update det."<<std::endl;
- #if VTK890
- ui->vtk->renderWindow()->Render();
- #else
- ui->vtk->GetRenderWindow()->Render();
- #endif
- int64_t nend = std::chrono::system_clock::now().time_since_epoch().count();
- int64_t nuse = nend- nstart;
- std::cout<<" draw use : "<<nuse/1e6<<std::endl;
- nlastup = std::chrono::system_clock::now().time_since_epoch().count();
- }
- void MainWindow::on_actionReset_Camera_triggered()
- {
- view->setCameraPosition(0,0,100,0,0,0,0,1,0);
- view->resetCamera(); //视角
- }
- void MainWindow::on_actionFull_Screem_triggered()
- {
- showFullScreen();
- ui->menubar->setVisible(false);
- ui->statusbar->setVisible(false);
- ui->toolBar->setVisible(false);
- mbFull = true;
- }
- void MainWindow::keyReleaseEvent(QKeyEvent *event)
- {
- if((event->key() == Qt::Key_Escape)||(event->key() == Qt::Key_F11))
- {
- if(mbFull)
- {
- showNormal();
- ui->menubar->setVisible(true);
- ui->statusbar->setVisible(true);
- ui->toolBar->setVisible(true);
- mbFull = false;
- }
- }
- }
- #ifdef USE_MODULECOMM
- void MainWindow::UpdateDet(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- (void)index;
- (void)dt;
- (void)strmemname;
- iv::radar::radar4ddetectarray xdetarray;
- if(xdetarray.ParseFromArray(strdata,(int)nSize))
- {
- mmutexdet.lock();
- mdetarray.CopyFrom(xdetarray);
- mbdetupdate = true;
- mmutexdet.unlock();
- emit detupdate();
- }
- else
- {
- std::cout<<"detection parse fail."<<std::endl;
- }
- }
- void MainWindow::UpdateObj(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- (void)index;
- (void)dt;
- (void)strmemname;
- iv::radar::radar4dobjectarray xobjarray;
- if(xobjarray.ParseFromArray(strdata,(int)nSize))
- {
- mmutexobj.lock();
- mobjarray.CopyFrom(xobjarray);
- mbobjupdate = true;
- mmutexobj.unlock();
- emit detupdate();
- }
- else
- {
- std::cout<<"object parse fail."<<std::endl;
- }
- }
- #else
- void MainWindow::threaddet()
- {
- while(mbThreadRun)
- {
- iv::radar::radar4ddetectarray xdetarray;
- if(mprecv->GetDetect(10,xdetarray) == 1)
- {
- mmutexdet.lock();
- mdetarray.CopyFrom(xdetarray);
- mbdetupdate = true;
- mmutexdet.unlock();
- emit detupdate();
- }
- }
- }
- void MainWindow::threadobj()
- {
- while(mbThreadRun)
- {
- iv::radar::radar4dobjectarray xobjarray;
- if(mprecv->GetObj(10,xobjarray) == 1)
- {
- mmutexobj.lock();
- mobjarray.CopyFrom(xobjarray);
- mbobjupdate = true;
- mmutexobj.unlock();
- emit detupdate();
- }
- }
- }
- #endif
- void MainWindow::on_actionAll_triggered()
- {
- if(ui->actionAll->isChecked()){
- ui->actionNoClassification->setChecked(true);
- ui->actionNoise->setChecked(true);
- ui->actionGround->setChecked(true);
- ui->actionTraversableUnder->setChecked(true);
- ui->actionObstacle->setChecked(true);
- ui->actionInvalid->setChecked(true);
- }
- else
- {
- ui->actionNoClassification->setChecked(false);
- ui->actionNoise->setChecked(false);
- ui->actionGround->setChecked(false);
- ui->actionTraversableUnder->setChecked(false);
- ui->actionObstacle->setChecked(false);
- ui->actionInvalid->setChecked(false);
- }
- }
- void MainWindow::on_actionObjectAll_triggered()
- {
- if(ui->actionObjectAll->isChecked()){
- ui->actionCar->setChecked(true);
- ui->actionTruck->setChecked(true);
- ui->actionMotorcycle->setChecked(true);
- ui->actionBicycle->setChecked(true);
- ui->actionPedestrian->setChecked(true);
- ui->actionAnimal->setChecked(true);
- ui->actionHazard->setChecked(true);
- ui->actionUnknown->setChecked(true);
- }
- else{
- ui->actionCar->setChecked(false);
- ui->actionTruck->setChecked(false);
- ui->actionMotorcycle->setChecked(false);
- ui->actionBicycle->setChecked(false);
- ui->actionPedestrian->setChecked(false);
- ui->actionAnimal->setChecked(false);
- ui->actionHazard->setChecked(false);
- ui->actionUnknown->setChecked(false);
- }
- }
|