diff --git a/.circleci/config.yml b/.circleci/config.yml new file mode 100644 index 0000000..a21bf5e --- /dev/null +++ b/.circleci/config.yml @@ -0,0 +1,18 @@ +version: 2 +jobs: + build: + docker: + - image: louisgcr/point_cloud_annotation_tool:latest + working_directory: /home + + steps: + - run: + shell: /bin/bash + name: Set-up repo and compile + command: | + git clone https://github.com/Louisgcr/point-cloud-annotation-tool.git + cd point-cloud-annotation-tool + mkdir build + cd build + cmake .. + make \ No newline at end of file diff --git a/.gitignore b/.gitignore index 4cabb45..415392e 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +docker/ build/ *.pcd *.user diff --git a/Annotaion.cpp b/Annotaion.cpp index a315ac6..98670af 100644 --- a/Annotaion.cpp +++ b/Annotaion.cpp @@ -2,6 +2,7 @@ #include #include #include +#include using namespace std; @@ -41,7 +42,6 @@ Annotation::Annotation(const PointCloudTPtr cloud, vector &slice, string ty vtkSmartPointer cubeTransform = vtkSmartPointer::New(); cubeTransform->PostMultiply(); cubeTransform->Scale(label.detail.length,label.detail.width,label.detail.height); - // cubeTransform->RotateZ(label.detail.yaw); cubeTransform->RotateZ(label.detail.yaw*180/vtkMath::Pi()); cubeTransform->Translate(label.detail.center_x,label.detail.center_y,label.detail.center_z); @@ -85,18 +85,12 @@ void Annotation::picked(vtkRenderWindowInteractor *interactor){ boxWidget = vtkSmartPointer::New(); boxWidgetCallback0 = vtkSmartPointer::New(); boxWidgetCallback0->setAnno(this); - boxWidgetCallback1 = vtkSmartPointer::New(); - boxWidgetCallback1->setAnno(this); + //boxWidgetCallback1 = vtkSmartPointer::New(); + //boxWidgetCallback1->setAnno(this); boxWidget->SetInteractor(interactor); - // boxWidget->SetProp3D( cubeActor ); - // boxWidget->SetPlaceFactor( 1.25 ); // Make the box 1.25x larger than the actor - // boxWidget->PlaceWidget(); - - // default is [-0.5, 0.5], NOTE - // [-1,1] makes boxwidget fit to annotion, - // but [-0.5, 0.5] should be in the correct way, may be some bug - double bounds[6]={-1,1,-1,1,-1,1}; + + double bounds[6]={-1,1,-1,1,-1,1}; boxWidget->PlaceWidget(bounds); vtkSmartPointer t=vtkSmartPointer::New(); @@ -106,8 +100,21 @@ void Annotation::picked(vtkRenderWindowInteractor *interactor){ boxWidget->GetOutlineProperty()->SetAmbientColor(1.0,0.0,0.0); boxWidget->AddObserver( vtkCommand::InteractionEvent, boxWidgetCallback0 ); - boxWidget->AddObserver( vtkCommand::EndInteractionEvent, boxWidgetCallback1 ); + //boxWidget->AddObserver( vtkCommand::EndInteractionEvent, boxWidgetCallback1 ); boxWidget->On(); + +} + +double* Annotation::getBoxPose(void){ + + return transform->GetPosition(); + +} + +double* Annotation::getBoxOri(void){ + + return transform->GetOrientation(); + } void Annotation::unpicked(){ @@ -217,7 +224,7 @@ void Annotation::colorAnnotation(int color_index){ actor->GetProperty()->SetLineWidth(2); actor->GetProperty ()->SetLighting (false); - + actor->GetProperty()->SetOpacity(0.5); mapper->SetLookupTable(lut); } @@ -348,9 +355,15 @@ void Annotaions::loadAnnotations(string filename) void Annotaions::saveAnnotations(string filename) { if (annotations.empty()) return; + + if(std::remove(filename.c_str()) == 0){ + std::cout<<"Removed old files"<getBoxLabel().toString()<getBoxLabel().toString()< / +........`-----' + +## License +[MIT](https://github.com/nishanths/license/blob/master/LICENSE) + + + diff --git a/common.h b/common.h old mode 100755 new mode 100644 diff --git a/example1.png b/example1.png deleted file mode 100644 index 147fbad..0000000 Binary files a/example1.png and /dev/null differ diff --git a/example2.png b/example2.png deleted file mode 100644 index 0c9101f..0000000 Binary files a/example2.png and /dev/null differ diff --git a/example_new.png b/example_new.png new file mode 100644 index 0000000..f5482d8 Binary files /dev/null and b/example_new.png differ diff --git a/main.cpp b/main.cpp old mode 100755 new mode 100644 diff --git a/mainwindow.ui b/mainwindow.ui old mode 100755 new mode 100644 index fd8042d..099b9d9 --- a/mainwindow.ui +++ b/mainwindow.ui @@ -95,7 +95,7 @@ 16777215 - 15 + 60 diff --git a/pcl/visualization/MyCloudLUT.cpp b/pcl/visualization/MyCloudLUT.cpp old mode 100755 new mode 100644 index fa8ac70..11fd58a --- a/pcl/visualization/MyCloudLUT.cpp +++ b/pcl/visualization/MyCloudLUT.cpp @@ -4,10 +4,10 @@ /// Lookup table static const unsigned char CloudLUT[] = { - 255 , 255, 255 , // default + 125 , 125, 125 , // default 255, 0, 0 , // highted - 0 , 0, 255 // ground - + 0 , 0, 255 , // ground + 0 , 255, 0 }; /// Number of colors in Glasbey lookup table diff --git a/pcl/visualization/MyCloudLUT.h b/pcl/visualization/MyCloudLUT.h old mode 100755 new mode 100644 diff --git a/pcl/visualization/PointCloudColorHandlerLUT.h b/pcl/visualization/PointCloudColorHandlerLUT.h old mode 100755 new mode 100644 diff --git a/pcl/visualization/pcl_visualizer_extented.h b/pcl/visualization/pcl_visualizer_extented.h old mode 100755 new mode 100644 diff --git a/pcl_demo.gif b/pcl_demo.gif new file mode 100644 index 0000000..7950c3a Binary files /dev/null and b/pcl_demo.gif differ diff --git a/visualizer.cpp b/visualizer.cpp old mode 100755 new mode 100644 index 842fadc..c6a8568 --- a/visualizer.cpp +++ b/visualizer.cpp @@ -15,6 +15,11 @@ #include #include #include +#include +#include +#include +#include +#include #include #include #include @@ -23,455 +28,785 @@ #include #include +#include + using namespace pcl::visualization; using namespace std; -//#define connect(...) QObject::connect(__VA_ARGS__); #define DEFAULT_POINT 0 #define SELECTED_POINT 1 #define GROUND_POINT 2 +#define INBOX_POINT 3 Visualizer::Visualizer(QWidget *parent) - :QMainWindow(parent), - ui(new Ui::MainWindow){ - - ui->setupUi(this); - this->setWindowTitle("3D annotation tool"); - - initialize(); - - // register selection http://www.pcl-users.org/Select-set-of-points-using-mouse-td3424113.html - - viewer->registerKeyboardCallback(boost::bind(&Visualizer::KeyboardEventProcess,this,_1)); - viewer->registerAreaPickingCallback(boost::bind(&Visualizer::AreaPickingEventProcess,this,_1)); - viewer->registerMouseCallback (boost::bind(&Visualizer::MouseEventProcess,this,_1)); - - // UI - connect(ui->action_Open,&QAction::triggered,this,&Visualizer::openFile); - connect(ui->action_Save,&QAction::triggered,this,&Visualizer::save); - connect(ui->action_detect_plane,&QAction::triggered,ui->PlaneDetect_dockWidget,&QDockWidget::show); - connect(ui->action_Threshold,&QAction::triggered,ui->threshold_dockWidget,&QDockWidget::show); - - ui->PlaneDetect_dockWidget->hide(); - ui->distanceThreshold_lineEdit->setText("0.1"); - ui->distanceThreshold_lineEdit->setValidator( new QDoubleValidator(-5, 5, 2) ); - connect(ui->applyPlaneDetection_pushButton ,&QPushButton::clicked,this,&Visualizer::planeDetect); - - ui->threshold_dockWidget->hide(); - ui->threshold_lineEdit->setText("-1.5"); - ui->threshold_lineEdit->setValidator( new QDoubleValidator(-100, 100, 2) ); - connect(ui->threshold_pushButton ,&QPushButton::clicked,this,&Visualizer::threshold); + :QMainWindow(parent), + ui(new Ui::MainWindow){ + + ui->setupUi(this); + this->setWindowTitle("3D annotation tool"); + + initialize(); + + // register selection http://www.pcl-users.org/Select-set-of-points-using-mouse-td3424113.html + viewer->registerKeyboardCallback(boost::bind(&Visualizer::KeyboardEventProcess,this,_1)); + viewer->registerAreaPickingCallback(boost::bind(&Visualizer::AreaPickingEventProcess,this,_1)); + viewer->registerMouseCallback (boost::bind(&Visualizer::MouseEventProcess,this,_1)); + + //Link ConfigureEventProcess to interactor ConfigureEvent to update message box with the current selected boxwidget + ui->qvtkWidget->GetInteractor()->AddObserver(vtkCommand::ConfigureEvent, this, &Visualizer::ConfigureEventProcess); + ui->qvtkWidget->update(); + + // UI + connect(ui->action_Open,&QAction::triggered,this,&Visualizer::openFile); + connect(ui->action_Save,&QAction::triggered,this,&Visualizer::save); + connect(ui->action_detect_plane,&QAction::triggered,ui->PlaneDetect_dockWidget,&QDockWidget::show); + connect(ui->action_Threshold,&QAction::triggered,ui->threshold_dockWidget,&QDockWidget::show); + + ui->PlaneDetect_dockWidget->hide(); + ui->distanceThreshold_lineEdit->setText("0.1"); + ui->distanceThreshold_lineEdit->setValidator( new QDoubleValidator(-5, 5, 2) ); + connect(ui->applyPlaneDetection_pushButton ,&QPushButton::clicked,this,&Visualizer::planeDetect); + + ui->threshold_dockWidget->hide(); + ui->threshold_lineEdit->setText("-1.5"); + ui->threshold_lineEdit->setValidator( new QDoubleValidator(-100, 100, 2) ); + connect(ui->threshold_pushButton ,&QPushButton::clicked,this,&Visualizer::threshold); + + //Flags to toggle between fullscreen and normal mode + fullscreen = false; + //Mouse click points + currentx = 0.0; + currenty = 0.0; + //Timer to prevent multiple button press for full screen + start = std::clock(); } Visualizer::~Visualizer(){ - delete ui; + delete ui; } +/* @brief: Helper initialize function. Creates the UI layout, render windows & axes widget + * */ void Visualizer::initialize() { - // init viewer - viewer.reset(new PCLViewer("",false)); - // BUG under windows, this cause execution exception - ui->qvtkWidget->SetRenderWindow(viewer->getRenderWindow()); - viewer->setupInteractor(ui->qvtkWidget->GetInteractor(),ui->qvtkWidget->GetRenderWindow()); - ui->qvtkWidget->update(); - - // init label type - Annotation::getTypes()->clear(); - Annotation::getTypes()->push_back("dontCare"); - Annotation::getTypes()->push_back("cyclist"); - Annotation::getTypes()->push_back("pedestrian"); - Annotation::getTypes()->push_back("vehicle"); - Annotation::getTypes()->push_back("unknown"); - FlowLayout *layout = new FlowLayout(); - for(auto type : *(Annotation::getTypes())){ - QPushButton* button= new QPushButton(QString::fromStdString(type)); - connect(button,&QPushButton::clicked,this,[=]() { this->typeButtonClickedProcess(button->text().toStdString()); }); - - pcl::RGB c=Annotation::getColor(type); - QPalette pal = button->palette(); - pal.setColor(QPalette::Button, QColor(c.r,c.g,c.b,c.a)); - button->setAutoFillBackground(true); - button->setPalette(pal); - button->update(); - layout->addWidget(button); - } - ui->groupBox_Types->setLayout(layout); - ui->groupBox_Types->setMaximumWidth(200); - ui->groupBox_Types->setMaximumHeight(200); - - - //init annotation - annoManager.reset(new Annotaions()); - currPickedAnnotation=NULL; - - //axes - vtkSmartPointer axes = - vtkSmartPointer::New(); - axesWidget = vtkSmartPointer::New(); - axesWidget->SetOrientationMarker( axes ); -// axesWidget->SetInteractor( viewer->getInteractorStyle()->GetInteractor() ); - axesWidget->SetInteractor( viewer->getRenderWindowInteractor() ); - axesWidget->SetViewport( 0.0, 0.0, 0.2, 0.2 ); - axesWidget->SetEnabled( 1 ); - axesWidget->InteractiveOff(); - - // init cloud - cloud.reset(new PointCloudT); - cloudLabel=NULL; + // init viewer + viewer.reset(new PCLViewer("",false)); + // BUG under windows, this cause execution exception + ui->qvtkWidget->SetRenderWindow(viewer->getRenderWindow()); + viewer->setupInteractor(ui->qvtkWidget->GetInteractor(),ui->qvtkWidget->GetRenderWindow()); + ui->qvtkWidget->update(); + + // init label type + Annotation::getTypes()->clear(); + Annotation::getTypes()->push_back("dontCare"); + Annotation::getTypes()->push_back("cyclist"); + Annotation::getTypes()->push_back("pedestrian"); + Annotation::getTypes()->push_back("vehicle"); + Annotation::getTypes()->push_back("unknown"); + FlowLayout *layout = new FlowLayout(); + + for(auto type : *(Annotation::getTypes())){ + QPushButton* button= new QPushButton(QString::fromStdString(type)); + connect(button,&QPushButton::clicked,this,[=]() { this->typeButtonClickedProcess(button->text().toStdString()); }); + + pcl::RGB c=Annotation::getColor(type); + QPalette pal = button->palette(); + pal.setColor(QPalette::Button, QColor(c.r,c.g,c.b,c.a)); + button->setAutoFillBackground(true); + button->setPalette(pal); + button->update(); + layout->addWidget(button); + } + ui->groupBox_Types->setLayout(layout); + ui->groupBox_Types->setMaximumWidth(200); + ui->groupBox_Types->setMaximumHeight(200); + + //Creates 4 windows to view point clouds + width = 0.6; // Width of main view + epi = 0.001; // create boarders for views + + viewer->getRendererCollection()->InitTraversal(); + vtkRenderer* temp = NULL; + temp = viewer->getRendererCollection()->GetNextItem(); + temp->SetViewport(0.0, 0.0, width - epi, 1); + + int v1(1); + viewer->createViewPort (width + epi, 0.671, 1.0, 1.00, v1); + viewer->setBackgroundColor (.2, .2, .2, v1); + + int v2(2); + viewer->createViewPort (width + epi, 0.331, 1.0, 0.669, v2); + viewer->setBackgroundColor (.2, .2, .2, v2); + + int v3(3); + viewer->createViewPort (width + epi, 0.00, 1.0, 0.329, v3); + viewer->setBackgroundColor (.2, .2, .2, v3); + + //axes + vtkSmartPointer axes = + vtkSmartPointer::New(); + axesWidget = vtkSmartPointer::New(); + axesWidget->SetOrientationMarker( axes ); + axesWidget->SetInteractor( viewer->getRenderWindowInteractor() ); + axesWidget->SetViewport( 0.0, 0.0, 0.2, 0.2 ); + axesWidget->SetEnabled( 1 ); + axesWidget->InteractiveOff(); + + //init annotation + annoManager.reset(new Annotaions()); + currPickedAnnotation=NULL; + + // init cloud + cloud.reset(new PointCloudT); + cloudLabel=NULL; } -void Visualizer::refresh() -{ - cloudLabel=new int[cloud->size()]; - memset(cloudLabel, 0, cloud->size()*sizeof(int)); - - ui->label_filename->setText(QString::fromStdString(pointcloudFileName)); - colorHandler.setInputCloud(cloud); - colorHandler.setLabel(cloudLabel); - viewer->addPointCloud(cloud,colorHandler,"cloud",0); - -// // show cloud -// //http://www.pcl-users.org/how-to-add-color-to-a-xyz-cloud-td4040030.html -// pcl::visualization::PointCloudColorHandlerCustom colorHandler(cloud, 255, 255, 255); -// // pcl::visualization::PointCloudColorHandlerGenericField colorHandler(cloud, "intensity"); - - -// PointCloudColorHandlerLabelField colorHandler; -// colorHandler. - - viewer->resetCamera(); - ui->qvtkWidget->update(); - - // show annotation if exists - showAnnotation(); -} +/*---------------------------------- Annotation ---------------------------------------------*/ +/* @brief: Pick the annotation within screen. If an actor already exist, unselect it (this will only work in another render), + * else select another actor, else let the user pan the camera. + * If an actor is select, configure the top, front and side view to zero in on the actor + * @param x: x position of mouse click + * @param y: y position of mouse click + * */ void Visualizer::pickAnnotation(double x,double y){ - vtkSmartPointer picker = - vtkSmartPointer::New(); - picker->Pick(x, y, 0, viewer->getRendererCollection()->GetFirstRenderer()); - vtkActor* pickedActor = picker->GetActor(); - - if (currPickedAnnotation){ - currPickedAnnotation->unpicked(); - currPickedAnnotation=NULL; - } - // get the correspond annotation - currPickedAnnotation = annoManager->getAnnotation(pickedActor); - if (currPickedAnnotation){ - currPickedAnnotation->picked(viewer->getRenderWindowInteractor()); - } + vtkSmartPointer picker = + vtkSmartPointer::New(); + + //Change renderer based on where the mouse is clicked + vtkRenderer* renderer = viewer->getRenderWindow()->GetInteractor()->FindPokedRenderer(x, y); + picker->Pick(x, y, 0, renderer); + vtkActor* pickedActor = picker->GetActor(); + + // get the correspond annotation + Annotation *clicked = annoManager->getAnnotation(pickedActor); + if(clicked){ + + pointsOutBox(); + if(currPickedAnnotation != NULL){ + currPickedAnnotation->unpicked(); + } + + if(currPickedAnnotation == clicked){ + currPickedAnnotation = NULL; + return; + } + + camera1 = vtkSmartPointer::New(); + camera2 = vtkSmartPointer::New(); + camera3 = vtkSmartPointer::New(); + + currPickedAnnotation = clicked; + currPickedAnnotation->picked(viewer->getRenderWindowInteractor()); + viewer->getRenderWindow()->GetInteractor()->ConfigureEvent(); + + //Find the actor position, orientation + double *act_ori; + act_ori = currPickedAnnotation->getBoxOri(); + double x_ori, y_ori, z_ori; + x_ori = *act_ori; + y_ori = *(act_ori + 1); + z_ori = *(act_ori + 2); + + double *act_pos; + act_pos = currPickedAnnotation->getBoxPose(); + double x, y ,z; + x = *act_pos; + y = *(act_pos + 1); + z = *(act_pos + 2); + + //For each render, set the camera to face the actor + viewer->getRendererCollection()->InitTraversal(); + vtkRenderer* temp = NULL; + viewer->getRendererCollection()->GetNextItem(); + + //top view + temp = viewer->getRendererCollection()->GetNextItem(); + camera1->SetPosition(x,y,z+15); + camera1->SetFocalPoint(x,y,z); + camera1->SetRoll(-z_ori); + temp->SetActiveCamera(camera1); + + //Front_view + double front_x = cos((vtkMath::Pi()*z_ori)/180.0)*(10.0) + x; + double front_y = sin((vtkMath::Pi()*z_ori)/180.0)*(10.0) + y; + temp = viewer->getRendererCollection()->GetNextItem(); + camera2->SetPosition(front_x,front_y,z); + camera2->SetFocalPoint(x,y,z); + camera2->SetViewUp(0,0,1); + temp->SetActiveCamera(camera2); + + //Side view + double side_x = cos((vtkMath::Pi()*(z_ori - 90.0))/180.0)*(10.0) + x; + double side_y = sin((vtkMath::Pi()*(z_ori - 90.0))/180.0)*(10.0) + y; + temp = viewer->getRendererCollection()->GetNextItem(); + camera3->SetPosition(side_x,side_y,z); + camera3->SetFocalPoint(x,y,z); + camera3->SetViewUp(0,0,1); + temp->SetActiveCamera(camera3); + + pointsInBox(); + + } } -void Visualizer::highlightPoint(std::vector& slice) +/* @brief: Show all annotation to render + * */ +void Visualizer::showAnnotation() { - if (slice.size()<1) return; - - for (std::vector::iterator it = slice.begin(); it != slice.end(); it++) { - cloudLabel[*it]=SELECTED_POINT; - } + for (auto anno:annoManager->getAnnotations()){ + showAnnotation(anno); + } } +/* @brief: show specific annotation to renderer + * @param anno: annotation to show + * */ +void Visualizer::showAnnotation(const Annotation* anno){ + viewer->addActorToRenderer(anno->getActor()); +} -void Visualizer::defaultColorPoint(std::vector& slice) +/* @brief: remove all annotation to render + * */ +void Visualizer::removeAnnotation() { - if (slice.size()==0){ - memset(cloudLabel, DEFAULT_POINT, cloud->size()*sizeof(int)); - } - - for (std::vector::iterator it = slice.begin(); it != slice.end(); it++) { - cloudLabel[*it]=DEFAULT_POINT; - } + for (auto anno:annoManager->getAnnotations()){ + removeAnnotation(anno); + } } -void Visualizer::groundColorPoint(std::vector& slice) +/* @brief: remove specific annotation to renderer + * @param anno: annotation to remove + * */ +void Visualizer::removeAnnotation(Annotation *anno) { - if (slice.size()<1) return; + if (currPickedAnnotation){ + currPickedAnnotation->unpicked(); + currPickedAnnotation=NULL; + } - for (std::vector::iterator it = slice.begin(); it != slice.end(); it++) { - cloudLabel[*it]=GROUND_POINT; - } -} + annoManager->remove(anno); + viewer->removeActorFromRenderer(anno->getActor()); +} -void Visualizer::createAnnotationFromSelectPoints(string type) +/*---------------------- Creating Annotation -----------------------------*/ +/* @brief: create an annotation if button is pressed + * @param type: what type of labels + * */ +void Visualizer::typeButtonClickedProcess(string type) { - if (last_selected_slice.size()>3){ - Annotation* anno=new Annotation(cloud,last_selected_slice,type); - annoManager->push_back(anno); - showAnnotation(anno); - ui->qvtkWidget->update(); - }else{ - std::printf("%s","no points selected"); - } + if (currPickedAnnotation){ + currPickedAnnotation->setType(type); + ui->qvtkWidget->update(); + return; + } + + if (last_selected_slice.size()>3){ + createAnnotationFromSelectPoints(type); + } } -void Visualizer::typeButtonClickedProcess(string type) +/* @brief: From the list of points, create an annotation + * @param type: type of labels to create + * */ +void Visualizer::createAnnotationFromSelectPoints(string type) { - // QMessageBox::information(this, QString::fromStdString("information"),QString::fromStdString(type)); - if (currPickedAnnotation){ - currPickedAnnotation->setType(type); - ui->qvtkWidget->update(); - return; - } - - if (last_selected_slice.size()>3){ - createAnnotationFromSelectPoints(type); - } + if (last_selected_slice.size()>3){ + Annotation* anno=new Annotation(cloud,last_selected_slice,type); + annoManager->push_back(anno); + showAnnotation(anno); + ui->qvtkWidget->update(); + }else{ + std::printf("%s","no points selected"); + } } +/*---------------------Cloud Manupilation-----------------------------------------*/ +/* @brief: updates the clour the all the cloud in the various view ports + * */ void Visualizer::updateCloud() { - viewer->updatePointCloud(cloud,colorHandler,"cloud"); - ui->qvtkWidget->update(); + viewer->updatePointCloud(cloud,colorHandler,"cloud"); + viewer->updatePointCloud(cloud,colorHandler,"cloud1"); + viewer->updatePointCloud(cloud,colorHandler,"cloud2"); + viewer->updatePointCloud(cloud,colorHandler,"cloud3"); + ui->qvtkWidget->update(); } +/* @brief: helper to find the intersection of point cloud + * @param a: vector of points + * @param b: vector of points + * @return intersection point cloud + * */ vector intersectionVector(vector &a,vector &b){ - vector c; - sort(a.begin(), a.end()); - sort(b.begin(), b.end()); - set_intersection(a.begin(),a.end(),b.begin(),b.end(),back_inserter(c)); - return c; + vector c; + sort(a.begin(), a.end()); + sort(b.begin(), b.end()); + set_intersection(a.begin(),a.end(),b.begin(),b.end(),back_inserter(c)); + return c; } +/* @brief: helper to find the union of point cloud + * @param a: vector of points + * @param b: vector of points + * @return union point cloud + * */ vector unionVector(vector &a,vector &b){ - vector c; - set_union(a.begin(),a.end(),b.begin(),b.end(),back_inserter(c)); - return c; + vector c; + set_union(a.begin(),a.end(),b.begin(),b.end(),back_inserter(c)); + return c; } +/* @brief: helper to find the difference between point cloud + * @param a: vector of points + * @param b: vector of points + * @return diff point cloud + * */ vector diffVector(vector a,vector b){ - vector c; - sort(a.begin(), a.end()); - sort(b.begin(), b.end()); - set_difference(a.begin(),a.end(),b.begin(),b.end(),back_inserter(c)); - return c; + vector c; + sort(a.begin(), a.end()); + sort(b.begin(), b.end()); + set_difference(a.begin(),a.end(),b.begin(),b.end(),back_inserter(c)); + return c; } - +/* @brief: Select the are and highlight points. + * If shfit key is pressed, union the points + * If ctrl key is press, remove the points + * @param event: event triggered by pcl visualizer + * */ void Visualizer::AreaPickingEventProcess (const pcl::visualization::AreaPickingEvent& event) { - vector new_selected_slice; - event.getPointsIndices(new_selected_slice); + vector new_selected_slice; + event.getPointsIndices(new_selected_slice); + + if (new_selected_slice.empty()) return; + + int s=viewer->getRenderWindowInteractor()->GetShiftKey(); + int a=viewer->getRenderWindowInteractor()->GetControlKey(); + + // remove ground points + vector r; + for (auto x:new_selected_slice){ + if (cloudLabel[x]!=GROUND_POINT){ + r.push_back(x); + } + } + new_selected_slice=r; + + if (!last_selected_slice.empty()){ + defaultColorPoint(last_selected_slice); + } + + if (s && a ){ // intersection + last_selected_slice=intersectionVector(last_selected_slice,new_selected_slice); + }else if (s){ // union + last_selected_slice=unionVector(last_selected_slice,new_selected_slice); + }else if (a){ // remove + last_selected_slice=diffVector(last_selected_slice,new_selected_slice); + }else{ // new + last_selected_slice=new_selected_slice; + } + + highlightPoint(last_selected_slice); + updateCloud(); +} - if (new_selected_slice.empty()) return; +/* @brief: find the points in the box, using the right side method. + * Find the vector of line from p1 p2 of bounding box and + * compute the dot product. If it is <= 0, then it is on the line + * and on the right side of the line. Repeate for all 4 side of box + * */ +void Visualizer::pointsInBox(void){ + + BoxLabel label; + label = currPickedAnnotation->getBoxLabel(); + double min_box_z = label.data[2] - label.data[5]/2.0; + double max_box_z = label.data[2] + label.data[5]/2.0; + + double sd = sin(label.data[6]); + double cd = cos(label.data[6]); + + double x1 = label.data[0] + (cd*(-label.data[3]/2.0) - sd*(-label.data[4]/2.0)); + double y1 = label.data[1] + (sd*(-label.data[3]/2.0) + cd*(-label.data[4]/2.0)); + + double x2 = label.data[0] + (cd*(-label.data[3]/2.0) - sd*(label.data[4]/2.0)); + double y2 = label.data[1] + (sd*(-label.data[3]/2.0) + cd*(label.data[4]/2.0)); + + double x3 = label.data[0] + (cd*(label.data[3]/2.0) - sd*(label.data[4]/2.0)); + double y3 = label.data[1] + (sd*(label.data[3]/2.0) + cd*(label.data[4]/2.0)); + + double x4 = label.data[0] + (cd*(label.data[3]/2.0) - sd*(-label.data[4]/2.0)); + double y4 = label.data[1] + (sd*(label.data[3]/2.0) + cd*(-label.data[4]/2.0)); + + vector slice; + for (int i=0;isize();i++){ + //If Point in current selected bounding box + double s1 = (x2 - x1)*(cloud->points[i].y - y1) - (cloud->points[i].x - x1)*(y2 - y1); + double s2 = (x3 - x2)*(cloud->points[i].y - y2) - (cloud->points[i].x - x2)*(y3 - y2); + double s3 = (x4 - x3)*(cloud->points[i].y - y3) - (cloud->points[i].x - x3)*(y4 - y3); + double s4 = (x1 - x4)*(cloud->points[i].y - y4) - (cloud->points[i].x - x4)*(y1 - y4); + + if( s1 <= 0.0 && s2 <=0.0 && s3<=0.0 && s4<=0.0 && + cloud->points[i].z > min_box_z && cloud->points[i].z < max_box_z){ + slice.push_back(i); + } + } + memset(cloudLabel, DEFAULT_POINT, cloud->size()*sizeof(int)); + inboxColorPoint(slice); + updateCloud(); +} - int s=viewer->getRenderWindowInteractor()->GetShiftKey(); - int a=viewer->getRenderWindowInteractor()->GetControlKey(); - // remove ground points - vector r; - for (auto x:new_selected_slice){ - if (cloudLabel[x]!=GROUND_POINT){ - r.push_back(x); - } - } - new_selected_slice=r; +void Visualizer::pointsOutBox(void){ + vector slice; + for (int i=0;isize();i++){ + slice.push_back(i); + } + memset(cloudLabel, DEFAULT_POINT, cloud->size()*sizeof(int)); + defaultColorPoint(slice); + updateCloud(); - if (!last_selected_slice.empty()){ - defaultColorPoint(last_selected_slice); - } +} +/*---------------------------------------------Color points----------------------------------------------*/ +/* @brief: highlights points that are selected + * @param: indexes of points selected + * */ +void Visualizer::highlightPoint(std::vector& slice) +{ + if (slice.size()<1) return; - if (s && a ){ // intersection - last_selected_slice=intersectionVector(last_selected_slice,new_selected_slice); - }else if (s){ // union - last_selected_slice=unionVector(last_selected_slice,new_selected_slice); - }else if (a){ // remove - last_selected_slice=diffVector(last_selected_slice,new_selected_slice); - }else{ // new - last_selected_slice=new_selected_slice; - } + for (std::vector::iterator it = slice.begin(); it != slice.end(); it++) { + cloudLabel[*it]=SELECTED_POINT; + } +} +/* @brief: make all the points the default color + * @param: indexes of points selected + * */ +void Visualizer::defaultColorPoint(std::vector& slice) +{ + if (slice.size()==0){ + memset(cloudLabel, DEFAULT_POINT, cloud->size()*sizeof(int)); + } - highlightPoint(last_selected_slice); - updateCloud(); + for (std::vector::iterator it = slice.begin(); it != slice.end(); it++) { + cloudLabel[*it]=DEFAULT_POINT; + } } - -void Visualizer::MouseEventProcess (const pcl::visualization::MouseEvent& event) +/* @brief: make all the points selected ground color + * @param: indexes of points selected + * */ +void Visualizer::groundColorPoint(std::vector& slice) { - if (event.getButton()==pcl::visualization::MouseEvent::LeftButton - && event.getType()==pcl::visualization::MouseEvent::MouseButtonPress){ - pickAnnotation(event.getX(), event.getY()); - } + if (slice.size()<1) return; + + for (std::vector::iterator it = slice.begin(); it != slice.end(); it++) { + cloudLabel[*it]=GROUND_POINT; + } } -void Visualizer::KeyboardEventProcess(const KeyboardEvent& event) +/* @brief: make all the points within a bounding box green color + * @param: indexes of points selected + * */ +void Visualizer::inboxColorPoint(std::vector& slice) { - std::cout<::iterator it = slice.begin(); it != slice.end(); it++) { + cloudLabel[*it]=INBOX_POINT; + } } -void Visualizer::showAnnotation() +/*-------------------------------- interactor events------------------------------------------*/ +void Visualizer::MouseEventProcess (const pcl::visualization::MouseEvent& event) { - for (auto anno:annoManager->getAnnotations()){ - showAnnotation(anno); - } -} + if (event.getButton()==pcl::visualization::MouseEvent::LeftButton + && event.getType()==pcl::visualization::MouseEvent::MouseButtonPress){ -void Visualizer::threshold(){ - double threhold_; - if (ui->threshold_lineEdit->text().isEmpty()) return; - threhold_=ui->threshold_lineEdit->text().toDouble(); + pickAnnotation(event.getX(), event.getY()); - vector slice; - for (int i=0;isize();i++){ - if (cloud->points[i].zsize()*sizeof(int)); - groundColorPoint(slice); - updateCloud(); + currentx = event.getX(); + currenty = event.getY(); + } } -void Visualizer::planeDetect(){ - if (ui->distanceThreshold_lineEdit->text().isEmpty()) return; - double distanceThreshold=ui->distanceThreshold_lineEdit->text().toDouble(); - - pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); - pcl::PointIndices::Ptr inliers (new pcl::PointIndices); - // Create the segmentation object - pcl::SACSegmentation seg; - // Optional - seg.setOptimizeCoefficients (true); - // Mandatory - seg.setModelType (pcl::SACMODEL_PLANE); - seg.setMethodType (pcl::SAC_RANSAC); - seg.setDistanceThreshold (distanceThreshold); - seg.setInputCloud (cloud); - seg.segment (*inliers, *coefficients); - - std::cout<<"plane detection: "<indices.size()<size()*sizeof(int)); - groundColorPoint(inliers->indices); - updateCloud(); - - - - -// PointCloudT::Ptr groundPlaneCloud; -// groundPlaneCloud.reset(new PointCloudT); - -// // http://docs.pointclouds.org/trunk/classpcl_1_1_extract_indices.html -// pcl::ExtractIndices eifilter (true); // Initializing with true will allow us to extract the removed indices -// eifilter.setInputCloud (cloud); -// eifilter.setIndices (inliers); -// eifilter.filter (*groundPlaneCloud); - -// // PointCloudT::Ptr groundPlaneCloud; -// // groundPlaneCloud.reset(new PointCloudT); -// // for (size_t i = 0; i < inliers->indices.size (); ++i){ -// // groundPlaneCloud->push_back(cloud->points[inliers->indices[i]]); -// // } - -// viewer->removeAllPointClouds(); -// ui->qvtkWidget->update(); -// if (ui->removePlane_checkBox->isChecked()){ -// pcl::visualization::PointCloudColorHandlerCustom colorHandler1(remainCloud, colors.at(1).r,colors.at(1).g,colors.at(1).b); -// viewer->addPointCloud(remainCloud,colorHandler1,"remainCloud"); -// ui->qvtkWidget->update(); -// }else{ -// pcl::visualization::PointCloudColorHandlerCustom colorHandler1(remainCloud, colors.at(1).r,colors.at(1).g,colors.at(1).b); -// pcl::visualization::PointCloudColorHandlerCustom colorHandler2(groundPlaneCloud, colors.at(2).r,colors.at(2).g,colors.at(2).b); -// viewer->addPointCloud(remainCloud,colorHandler1,"remainCloud"); -// viewer->addPointCloud(groundPlaneCloud,colorHandler2,"groundPlaneCloud"); -// ui->qvtkWidget->update(); -// } +/* @brief: events that triggers when actor is selected and moving the handles + * triggers to update label message box + * */ +void Visualizer::ConfigureEventProcess(){ + + if(currPickedAnnotation != NULL){ + + BoxLabel label; + label = currPickedAnnotation->getBoxLabel(); + + std::string temp[7]; + for(int i=0; i <7; i++){ + temp[i] = std::to_string(label.data[i]); + temp[i].resize(7); + } + std::string str = "X:" + temp[0] + " Lenght:" + temp[3]+ + "\r\nY:" + temp[1] + " Width :" + temp[4]+ + "\r\nZ:" + temp[2] + " Height:" + temp[5]+ + "\r\nYaw:" + temp[6] + " deg"; + + ui->label_message->setText(QString::fromStdString(str)); + } } -void Visualizer::openFile() +void Visualizer::KeyboardEventProcess(const KeyboardEvent& event) { - pointcloudFileName = QFileDialog::getOpenFileName(this, tr("Open PCD file"), "/home/fancy", tr("PCD Files (*.pcd *.bin)")).toStdString(); - if (pointcloudFileName.empty()) return; - - clear(); - QFileInfo file(QString::fromStdString(pointcloudFileName)); - QString ext = file.completeSuffix(); // ext = "bin" ,"pcd" - if (ext=="pcd"){ - pcl::io::loadPCDFile(pointcloudFileName,*cloud); - }else{ - loadBinFile(pointcloudFileName,*cloud); - } + // delete annotation + if (event.getKeySym()=="Delete" && currPickedAnnotation){ + removeAnnotation(currPickedAnnotation); + } + + //Toggle fullscreen between renders + int c=viewer->getRenderWindowInteractor()->GetControlKey(); + char* z=viewer->getRenderWindowInteractor()->GetKeySym(); + + duration = ((std::clock() - start)*1000.0) / (double) CLOCKS_PER_SEC; + if (*z=='z' && c && duration > 35.0){ + start = std::clock(); + toggleRenderers(); + } - std::cout<width*cloud->height<loadAnnotations(annotationFileName); - } +} - refresh(); +/*------------------------------------------File related functions--------------------------------------------------*/ +/* @brief: Open files. Only accpts pcd and bin fils for point cloud and txt file for annotation. + * Prints out instruction for user + * */ +void Visualizer::openFile() +{ + //change location of open file to user desktop + string path(getenv("HOME")); + path += "/Desktop/"; + QString qstr = QString::fromStdString(path); + + pointcloudFileName = QFileDialog::getOpenFileName(this, tr("Open PCD file"), qstr, tr("PCD Files (*.pcd *.bin)")).toStdString(); + if (pointcloudFileName.empty()) return; + + viewer->removeAllShapes(); + clear(); + + QFileInfo file(QString::fromStdString(pointcloudFileName)); + QString ext = file.completeSuffix(); // ext = "bin" ,"pcd" + + if (ext=="pcd"){ + pcl::io::loadPCDFile(pointcloudFileName,*cloud); + }else{ + loadBinFile(pointcloudFileName,*cloud); + } + + std::cout<width*cloud->height<loadAnnotations(annotationFileName); + }else{ + //Try load from previous PCD + string previous_file_name = pointcloudFileName; + + previous_file_name.erase(previous_file_name.find(".pcd"), 4); + std::size_t found = previous_file_name.find_last_of("/\\"); + + std::string sub1 = previous_file_name.substr(0, found+1); + std::string sub2 = previous_file_name.substr(found+1); + + int temp = std::stoi(sub2); + temp -= 1; + + annotationFileName= sub1 + std::to_string(temp) + ".pcd.txt"; + std::cout<loadAnnotations(annotationFileName); + } + annotationFileName=pointcloudFileName+".txt"; + } + + refresh(); + std::cout<<"Instructions: "<removeAllPointClouds(); - annoManager->clear(); + removeAnnotation(); + viewer->removeAllPointClouds(); + annoManager->clear(); - currPickedAnnotation=NULL; - last_selected_slice.clear(); + currPickedAnnotation=NULL; + last_selected_slice.clear(); - if (cloudLabel){ - delete [] cloudLabel; - cloudLabel=NULL; - } + if (cloudLabel){ + delete [] cloudLabel; + cloudLabel=NULL; + } } +/* @brief: when a new file is loaded, it reinit all the point cloud and annotations + * */ +void Visualizer::refresh() +{ + cloudLabel=new int[cloud->size()]; + memset(cloudLabel, 0, cloud->size()*sizeof(int)); + + ui->label_filename->setText(QString::fromStdString(pointcloudFileName)); + colorHandler.setInputCloud(cloud); + colorHandler.setLabel(cloudLabel); + + viewer->addPointCloud(cloud,colorHandler,"cloud",0); + viewer->addPointCloud(cloud,colorHandler,"cloud1",1); + viewer->addPointCloud(cloud,colorHandler,"cloud2",2); + viewer->addPointCloud(cloud,colorHandler,"cloud3",3); + + //show annotation if exists + showAnnotation(); + + viewer->resetCamera(); + ui->qvtkWidget->update(); +} + +/* @brief: save annotation to current point cloud location + * */ void Visualizer::save() { - annoManager->saveAnnotations(annotationFileName); + annoManager->saveAnnotations(annotationFileName); } +/* @brief: load binary point cloud files + * */ void Visualizer::loadBinFile(string filename_,PointCloudT& cloud_) { - std::ifstream input(filename_.c_str(), std::ios_base::binary); - if(!input.good()){ - std::cerr<<"Cannot open file : "<addActorToRenderer(anno->getActor()); +/*----------------------------------------------------------- Extended Functions ------------------------------------------------*/ +/*@brief: Gets the threshold value set from the UI*/ +void Visualizer::threshold(){ + double threhold_; + if (ui->threshold_lineEdit->text().isEmpty()) return; + threhold_=ui->threshold_lineEdit->text().toDouble(); + + vector slice; + for (int i=0;isize();i++){ + if (cloud->points[i].zsize()*sizeof(int)); + groundColorPoint(slice); + updateCloud(); } -void Visualizer::removeAnnotation() -{ - for (auto anno:annoManager->getAnnotations()){ - removeAnnotation(anno); - } +/* @brief: when button to detect plane is click, it trys to find the ground plane*/ +void Visualizer::planeDetect(){ + if (ui->distanceThreshold_lineEdit->text().isEmpty()) return; + double distanceThreshold=ui->distanceThreshold_lineEdit->text().toDouble(); + + pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); + pcl::PointIndices::Ptr inliers (new pcl::PointIndices); + // Create the segmentation object + pcl::SACSegmentation seg; + // Optional + seg.setOptimizeCoefficients (true); + // Mandatory + seg.setModelType (pcl::SACMODEL_PLANE); + seg.setMethodType (pcl::SAC_RANSAC); + seg.setDistanceThreshold (distanceThreshold); + seg.setInputCloud (cloud); + seg.segment (*inliers, *coefficients); + + memset(cloudLabel, DEFAULT_POINT, cloud->size()*sizeof(int)); + groundColorPoint(inliers->indices); + updateCloud(); + } -void Visualizer::removeAnnotation(Annotation *anno) -{ - if (currPickedAnnotation){ - currPickedAnnotation->unpicked(); - currPickedAnnotation=NULL; - } - viewer->removeActorFromRenderer(anno->getActor()); +/*--------------------------------View port Manipulation--------------------------------------------*/ +/* @brief: Toggles the viewport from full screen to zoomed in view of the selected viewport + * */ +void Visualizer::toggleRenderers(void){ + + if(fullscreen){ + fullscreen = false; + + viewer->getRendererCollection()->InitTraversal(); + vtkRenderer* temp = NULL; + temp = viewer->getRendererCollection()->GetNextItem(); + temp->SetViewport(0.0, 0.0, width - epi, 1); + temp->SetBackground (.0, .0, .0); + + temp = viewer->getRendererCollection()->GetNextItem(); + temp->SetViewport (width + epi, 0.67, 1.0, 1.00); + temp->SetBackground (.2, .2, .2); + + temp = viewer->getRendererCollection()->GetNextItem(); + temp->SetViewport(width + epi, 0.330, 1.0, 0.67); + temp->SetBackground (.25, .25, .25); + + temp = viewer->getRendererCollection()->GetNextItem(); + temp->SetViewport (width + epi, 0.00, 1.0, 0.33); + temp->SetBackground (.2, .2, .2); + + + }else{ + //Find the current renderer + vtkRenderer* renderer = viewer->getRenderWindow()->GetInteractor()->FindPokedRenderer(currentx, currenty); + + viewer->getRendererCollection()->InitTraversal(); + vtkRenderer* temp = NULL; + + for(int i =0; i < 4; i ++){ + + temp = viewer->getRendererCollection()->GetNextItem(); + if(temp == renderer){ + temp->SetViewport (0.0, 0.0, 1., 1.); + }else{ + temp->SetViewport (0.0, 0.0, 0.001, 0.001); + } + } + + fullscreen = true; + } + } + diff --git a/visualizer.h b/visualizer.h old mode 100755 new mode 100644 index 398dbe0..e132cf0 --- a/visualizer.h +++ b/visualizer.h @@ -20,16 +20,26 @@ class Visualizer:public QMainWindow void highlightPoint(vector& slice); void defaultColorPoint(vector& slice); void groundColorPoint(vector& slice); + void inboxColorPoint(vector& slice); void pickAnnotation(double x,double y); void createAnnotationFromSelectPoints(string type="unknown"); void typeButtonClickedProcess(string type); void updateCloud(); - + double* getBoxPose(void); + void pointsInBox(void); + void pointsOutBox(void); protected: Ui::MainWindow* ui; PCLViewerPtr viewer; string pointcloudFileName; string annotationFileName; + + bool fullscreen; + double width; + double epi; + double currentx; + double currenty; + /** * @brief the loaded cloud */ @@ -78,6 +88,7 @@ class Visualizer:public QMainWindow private: void AreaPickingEventProcess(const pcl::visualization::AreaPickingEvent& event); void MouseEventProcess (const pcl::visualization::MouseEvent& event); + void ConfigureEventProcess(void); void KeyboardEventProcess(const pcl::visualization::KeyboardEvent& event); // visibility @@ -85,10 +96,16 @@ class Visualizer:public QMainWindow void showAnnotation(const Annotation* anno); void removeAnnotation(); void removeAnnotation(Annotation* anno); + void toggleRenderers(void); + std::clock_t start; + double duration; // axes vtkSmartPointer axesWidget; + vtkSmartPointer camera1; + vtkSmartPointer camera2; + vtkSmartPointer camera3; // for pick Annotation *currPickedAnnotation; }; diff --git a/vtkAnnotationBoxSource.cpp b/vtkAnnotationBoxSource.cpp old mode 100755 new mode 100644 diff --git a/vtkAnnotationBoxSource.h b/vtkAnnotationBoxSource.h old mode 100755 new mode 100644 diff --git a/vtkBoxWidgetCallback.cpp b/vtkBoxWidgetCallback.cpp index 28fd773..d687a1c 100644 --- a/vtkBoxWidgetCallback.cpp +++ b/vtkBoxWidgetCallback.cpp @@ -12,6 +12,7 @@ void vtkBoxWidgetCallback0::Execute(vtkObject *caller, unsigned long, void *) { // Here we use the vtkBoxWidget to transform the underlying coneActor // (by manipulating its transformation matrix). + //std::cout<<"VTK Box Widget CB 0 Execute"< t = vtkSmartPointer::New(); vtkBoxWidget *widget = reinterpret_cast(caller); @@ -36,7 +37,7 @@ vtkBoxWidgetCallback1 *vtkBoxWidgetCallback1::New() void vtkBoxWidgetCallback1::Execute(vtkObject *caller, unsigned long, void *) { - if (anno){ + if (anno){ anno->adjustToAnchor(); } } diff --git a/vtkBoxWidgetRestricted.cpp b/vtkBoxWidgetRestricted.cpp old mode 100755 new mode 100644 index 752e6e2..5489e0a --- a/vtkBoxWidgetRestricted.cpp +++ b/vtkBoxWidgetRestricted.cpp @@ -3,50 +3,154 @@ vtkStandardNewMacro(vtkBoxWidgetRestricted); void vtkBoxWidgetRestricted::Rotate(int X, int Y, double *p1, double *p2, double *vpn){ - double *pts = - static_cast(this->Points->GetData())->GetPointer(0); - double *center = - static_cast(this->Points->GetData())->GetPointer(3*14); - double v[3]; //vector of motion - // double axis[3]; //axis of rotation - double theta; //rotation angle - int i; - - v[0] = p2[0] - p1[0]; - v[1] = p2[1] - p1[1]; - v[2] = p2[2] - p1[2]; - - // Create axis of rotation and angle of rotation - // compute the vector which perpendicular to Z axis in current view - double z[3]={0,0,1}; //restrict to Z rotation - double m[3]; - vtkMath::Cross(z,vpn,m); - if ( vtkMath::Normalize(m) == 0.0 || vtkMath::Normalize(v) == 0.0) - { - return; - } - // alpha is the angle between the motion vector and vector m - double cos_alpha=vtkMath::Dot(m,v); -// std::cout<CurrentRenderer->GetSize(); - double l2 = (X-this->Interactor->GetLastEventPosition()[0])*(X-this->Interactor->GetLastEventPosition()[0]) + (Y-this->Interactor->GetLastEventPosition()[1])*(Y-this->Interactor->GetLastEventPosition()[1]); - theta = 360.0 * cos_alpha *sqrt(l2/(size[0]*size[0]+size[1]*size[1])); - - //Manipulate the transform to reflect the rotation - this->Transform->Identity(); - this->Transform->Translate(center[0],center[1],center[2]); - this->Transform->RotateZ(theta); - this->Transform->Translate(-center[0],-center[1],-center[2]); - - //Set the corners - vtkPoints *newPts = vtkPoints::New(VTK_DOUBLE); - this->Transform->TransformPoints(this->Points,newPts); - - for (i=0; i<8; i++, pts+=3) - { - this->Points->SetPoint(i, newPts->GetPoint(i)); - } - - newPts->Delete(); - this->PositionHandles(); + double *pts = + static_cast(this->Points->GetData())->GetPointer(0); + double *center = + static_cast(this->Points->GetData())->GetPointer(3*14); + double v[3]; //vector of motion + // double axis[3]; //axis of rotation + double theta; //rotation angle + int i; + + v[0] = p2[0] - p1[0]; + v[1] = p2[1] - p1[1]; + v[2] = p2[2] - p1[2]; + + // Create axis of rotation and angle of rotation + // compute the vector which perpendicular to Z axis in current view + double z[3]={0,0,1}; //restrict to Z rotation + double m[3]; + vtkMath::Cross(z,vpn,m); + if ( vtkMath::Normalize(m) == 0.0 || vtkMath::Normalize(v) == 0.0) + { + return; + } + // alpha is the angle between the motion vector and vector m + double cos_alpha=vtkMath::Dot(m,v); + // std::cout<CurrentRenderer->GetSize(); + double l2 = (X-this->Interactor->GetLastEventPosition()[0])*(X-this->Interactor->GetLastEventPosition()[0]) + (Y-this->Interactor->GetLastEventPosition()[1])*(Y-this->Interactor->GetLastEventPosition()[1]); + theta = 360.0 * cos_alpha *sqrt(l2/(size[0]*size[0]+size[1]*size[1])); + + //Manipulate the transform to reflect the rotation + this->Transform->Identity(); + this->Transform->Translate(center[0],center[1],center[2]); + this->Transform->RotateZ(theta); + this->Transform->Translate(-center[0],-center[1],-center[2]); + + //Set the corners + vtkPoints *newPts = vtkPoints::New(VTK_DOUBLE); + this->Transform->TransformPoints(this->Points,newPts); + + for (i=0; i<8; i++, pts+=3) + { + this->Points->SetPoint(i, newPts->GetPoint(i)); + } + + newPts->Delete(); + this->PositionHandles(); +} + +void vtkBoxWidgetRestricted::SizeHandles(void){ + + double radius = this->vtk3DWidget::SizeHandles(0.6); + + for(int i=0; i<7; i++) + { + this->HandleGeometry[i]->SetRadius(radius); + } + +} + + +void vtkBoxWidgetRestricted::OnMouseMove(void){ + + // See whether we're active + if ( this->State == vtkBoxWidget::Outside || + this->State == vtkBoxWidget::Start ) + { + return; + } + + int X = this->Interactor->GetEventPosition()[0]; + int Y = this->Interactor->GetEventPosition()[1]; + + // Do different things depending on state + // Calculations everybody does + double focalPoint[4], pickPoint[4], prevPickPoint[4]; + double z, vpn[3]; + + vtkCamera *camera = this->CurrentRenderer->GetActiveCamera(); + if ( !camera ) + { + return; + } + + + // Compute the two points defining the motion vector + this->ComputeWorldToDisplay(this->LastPickPosition[0], this->LastPickPosition[1], + this->LastPickPosition[2], focalPoint); + z = focalPoint[2]; + this->ComputeDisplayToWorld(double(this->Interactor->GetLastEventPosition()[0]), + double(this->Interactor->GetLastEventPosition()[1]), + z, prevPickPoint); + this->ComputeDisplayToWorld(double(X), double(Y), z, pickPoint); + + // Process the motion + if ( this->State == vtkBoxWidget::Moving ) + { + // Okay to process + if ( this->CurrentHandle ) + { + if ( this->RotationEnabled && this->CurrentHandle == this->HexFace ) + { + camera->GetViewPlaneNormal(vpn); + this->Rotate(X, Y, prevPickPoint, pickPoint, vpn); + } + else if ( this->TranslationEnabled && this->CurrentHandle == this->Handle[6] ) + { + this->Translate(prevPickPoint, pickPoint); + } + else if ( this->TranslationEnabled && this->ScalingEnabled ) + { + if ( this->CurrentHandle == this->Handle[0] ) + { + this->MoveMinusXFace(prevPickPoint, pickPoint); + } + else if ( this->CurrentHandle == this->Handle[1] ) + { + this->MovePlusXFace(prevPickPoint, pickPoint); + } + else if ( this->CurrentHandle == this->Handle[2] ) + { + this->MoveMinusYFace(prevPickPoint, pickPoint); + } + else if ( this->CurrentHandle == this->Handle[3] ) + { + this->MovePlusYFace(prevPickPoint, pickPoint); + } + else if ( this->CurrentHandle == this->Handle[4] ) + { + this->MoveMinusZFace(prevPickPoint, pickPoint); + } + else if ( this->CurrentHandle == this->Handle[5] ) + { + this->MovePlusZFace(prevPickPoint, pickPoint); + } + } + } + + + } + else if ( this->ScalingEnabled && this->State == vtkBoxWidget::Scaling ) + { + this->Scale(prevPickPoint, pickPoint, X, Y); + } + + // Interact, if desired + this->EventCallbackCommand->SetAbortFlag(1); + this->InvokeEvent(vtkCommand::InteractionEvent, nullptr); + this->Interactor->ConfigureEvent(); + this->Interactor->Render(); + } diff --git a/vtkBoxWidgetRestricted.h b/vtkBoxWidgetRestricted.h old mode 100755 new mode 100644 index a08b328..594ba22 --- a/vtkBoxWidgetRestricted.h +++ b/vtkBoxWidgetRestricted.h @@ -8,6 +8,9 @@ #include #include #include +#include "vtkSphereSource.h" +#include "vtkCamera.h" +#include "vtkCallbackCommand.h" /** * @brief The vtkBoxWidgetRestricted class @@ -23,5 +26,7 @@ class vtkBoxWidgetRestricted: public vtkBoxWidget{ vtkTypeMacro(vtkBoxWidgetRestricted,vtkBoxWidget); virtual void Rotate(int X, int Y, double *p1, double *p2, double *vpn); + virtual void SizeHandles(void); + virtual void OnMouseMove(void); }; #endif