00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #include <ctype.h>
00041 #include <QFileDialog>
00042 #include <QMessageBox>
00043 #include <QMouseEvent>
00044 #include <GL/gl.h>
00045 #include <GL/glu.h>
00046 #include <pcl/filters/filter.h>
00047 #include <pcl/io/pcd_io.h>
00048 #include <pcl/apps/point_cloud_editor/cloudEditorWidget.h>
00049 #include <pcl/apps/point_cloud_editor/common.h>
00050 #include <pcl/apps/point_cloud_editor/cloud.h>
00051 #include <pcl/apps/point_cloud_editor/cloudTransformTool.h>
00052 #include <pcl/apps/point_cloud_editor/selectionTransformTool.h>
00053 #include <pcl/apps/point_cloud_editor/selection.h>
00054 #include <pcl/apps/point_cloud_editor/select1DTool.h>
00055 #include <pcl/apps/point_cloud_editor/select2DTool.h>
00056
00057 #include <pcl/apps/point_cloud_editor/copyBuffer.h>
00058 #include <pcl/apps/point_cloud_editor/copyCommand.h>
00059 #include <pcl/apps/point_cloud_editor/pasteCommand.h>
00060 #include <pcl/apps/point_cloud_editor/deleteCommand.h>
00061 #include <pcl/apps/point_cloud_editor/denoiseCommand.h>
00062 #include <pcl/apps/point_cloud_editor/cutCommand.h>
00063 #include <pcl/apps/point_cloud_editor/mainWindow.h>
00064
00065 CloudEditorWidget::CloudEditorWidget (QWidget *parent)
00066 : QGLWidget(QGLFormat(QGL::DoubleBuffer | QGL::DepthBuffer |
00067 QGL::Rgba | QGL::StencilBuffer), parent),
00068 point_size_(2.0f), selected_point_size_(4.0f),
00069 cam_fov_(60.0), cam_aspect_(1.0), cam_near_(0.0001), cam_far_(100.0),
00070 color_scheme_(COLOR_BY_PURE), is_colored_(false)
00071 {
00072 setFocusPolicy(Qt::StrongFocus);
00073 command_queue_ptr_ = CommandQueuePtr(new CommandQueue());
00074 initFileLoadMap();
00075 initKeyMap();
00076 }
00077
00078 CloudEditorWidget::~CloudEditorWidget ()
00079 {
00080 }
00081
00082 void
00083 CloudEditorWidget::loadFile(const std::string &filename)
00084 {
00085 std::string ext = filename.substr(filename.find_last_of(".")+1);
00086 FileLoadMap::iterator it = cloud_load_func_map_.find(ext);
00087 if (it != cloud_load_func_map_.end())
00088 (it->second)(this, filename);
00089 else
00090 loadFilePCD(filename);
00091 }
00092
00093 void
00094 CloudEditorWidget::load ()
00095 {
00096 QString file_path = QFileDialog::getOpenFileName(this, tr("Open File"));
00097
00098 if (file_path.isEmpty())
00099 return;
00100
00101 try
00102 {
00103 loadFile(file_path.toStdString());
00104 }
00105 catch (...)
00106 {
00107 QMessageBox::information(this, tr("Point Cloud Editor"),
00108 tr("Can not load %1.").arg(file_path));
00109 }
00110 update();
00111 updateGL();
00112 }
00113
00114 void
00115 CloudEditorWidget::save ()
00116 {
00117 if (!cloud_ptr_)
00118 {
00119 QMessageBox::information(this, tr("Point Cloud Editor"),
00120 tr("No cloud is loaded."));
00121 return;
00122 }
00123
00124 QString file_path = QFileDialog::getSaveFileName(this,tr("Save point cloud"));
00125
00126 std::string file_path_std = file_path.toStdString();
00127 if ( (file_path_std == "") || (!cloud_ptr_) )
00128 return;
00129
00130 if (is_colored_)
00131 {
00132
00133 swapRBValues();
00134 try
00135 {
00136 pcl::io::savePCDFile(file_path_std, cloud_ptr_->getInternalCloud());
00137 }
00138 catch (...)
00139 {
00140 QMessageBox::information(this, tr("Point Cloud Editor"),
00141 tr("Can not save %1.").arg(file_path));
00142 }
00143 swapRBValues();
00144 }
00145 else
00146 {
00147 pcl::PointCloud<pcl::PointXYZ> uncolored_cloud;
00148 pcl::copyPointCloud(cloud_ptr_->getInternalCloud(), uncolored_cloud);
00149 try
00150 {
00151 pcl::io::savePCDFile(file_path_std, uncolored_cloud);
00152 }
00153 catch (...)
00154 {
00155 QMessageBox::information(this, tr("Point Cloud Editor"),
00156 tr("Can not save %1.").arg(file_path));
00157 }
00158 }
00159 }
00160
00161 void
00162 CloudEditorWidget::toggleBlendMode ()
00163 {
00164 if (!cloud_ptr_)
00165 return;
00166 GLint blend_src = 0;
00167 glGetIntegerv(GL_BLEND_SRC, &blend_src);
00168 if (blend_src == GL_SRC_ALPHA)
00169 glBlendFunc( GL_ONE, GL_ZERO );
00170 else
00171 glBlendFunc( GL_SRC_ALPHA, GL_ZERO );
00172 update();
00173 }
00174
00175 void
00176 CloudEditorWidget::view ()
00177 {
00178 if (!cloud_ptr_)
00179 return;
00180 tool_ptr_ = boost::shared_ptr<CloudTransformTool>(
00181 new CloudTransformTool(cloud_ptr_));
00182 }
00183
00184 void
00185 CloudEditorWidget::select1D ()
00186 {
00187 if (!cloud_ptr_)
00188 return;
00189 tool_ptr_ = boost::shared_ptr<Select1DTool>(new Select1DTool(selection_ptr_,
00190 cloud_ptr_));
00191 update();
00192 }
00193
00194 void
00195 CloudEditorWidget::select2D ()
00196 {
00197 if (!cloud_ptr_)
00198 return;
00199 tool_ptr_ = boost::shared_ptr<Select2DTool>(new Select2DTool(selection_ptr_,
00200 cloud_ptr_));
00201 update();
00202 }
00203
00204 void
00205 CloudEditorWidget::select3D ()
00206 {
00207 if (!cloud_ptr_)
00208 return;
00209
00210
00211 update();
00212 }
00213
00214 void
00215 CloudEditorWidget::invertSelect ()
00216 {
00217 if (!selection_ptr_)
00218 return;
00219 selection_ptr_ -> invertSelect();
00220 cloud_ptr_->setSelection(selection_ptr_);
00221 update();
00222 }
00223
00224 void
00225 CloudEditorWidget::cancelSelect ()
00226 {
00227 if (!selection_ptr_)
00228 return;
00229 selection_ptr_ -> clear();
00230 update();
00231 }
00232
00233 void
00234 CloudEditorWidget::copy ()
00235 {
00236 if (!cloud_ptr_)
00237 return;
00238 if (!selection_ptr_ || selection_ptr_->empty())
00239 return;
00240 boost::shared_ptr<CopyCommand> c(new CopyCommand(copy_buffer_ptr_,
00241 selection_ptr_, cloud_ptr_));
00242 command_queue_ptr_->execute(c);
00243 }
00244
00245 void
00246 CloudEditorWidget::paste ()
00247 {
00248 if (!cloud_ptr_)
00249 return;
00250 if (!copy_buffer_ptr_ || copy_buffer_ptr_->empty())
00251 return;
00252 boost::shared_ptr<PasteCommand> c(new PasteCommand(copy_buffer_ptr_,
00253 selection_ptr_, cloud_ptr_));
00254 command_queue_ptr_->execute(c);
00255 update();
00256 }
00257
00258 void
00259 CloudEditorWidget::remove ()
00260 {
00261 if (!cloud_ptr_)
00262 return;
00263 if (!selection_ptr_ || selection_ptr_->empty())
00264 return;
00265 boost::shared_ptr<DeleteCommand> c(new DeleteCommand(selection_ptr_,
00266 cloud_ptr_));
00267 command_queue_ptr_->execute(c);
00268 update();
00269 }
00270
00271 void
00272 CloudEditorWidget::cut ()
00273 {
00274 if (!cloud_ptr_)
00275 return;
00276 if (!selection_ptr_ || selection_ptr_->empty())
00277 return;
00278 boost::shared_ptr<CutCommand> c(new CutCommand(copy_buffer_ptr_,
00279 selection_ptr_, cloud_ptr_));
00280 command_queue_ptr_->execute(c);
00281 update();
00282 }
00283
00284 void
00285 CloudEditorWidget::transform ()
00286 {
00287 if (!cloud_ptr_ || !selection_ptr_ || selection_ptr_->empty())
00288 return;
00289 tool_ptr_ = boost::shared_ptr<SelectionTransformTool>(
00290 new SelectionTransformTool(selection_ptr_, cloud_ptr_, command_queue_ptr_));
00291 update();
00292 }
00293
00294 void
00295 CloudEditorWidget::denoise ()
00296 {
00297 if (!cloud_ptr_)
00298 return;
00299 DenoiseParameterForm form;
00300 form.exec();
00301 boost::shared_ptr<DenoiseCommand> c(new DenoiseCommand(selection_ptr_,
00302 cloud_ptr_, form.getMeanK(), form.getStdDevThresh()));
00303 command_queue_ptr_->execute(c);
00304 update();
00305 }
00306
00307 void
00308 CloudEditorWidget::undo ()
00309 {
00310 if (!cloud_ptr_)
00311 return;
00312 command_queue_ptr_ -> undo();
00313 update();
00314 }
00315
00316 void
00317 CloudEditorWidget::increasePointSize ()
00318 {
00319 ((MainWindow*) parentWidget()) -> increaseSpinBoxValue();
00320 point_size_ = ((MainWindow*) parentWidget()) -> getSpinBoxValue();
00321 if (!cloud_ptr_)
00322 return;
00323 cloud_ptr_->setPointSize(point_size_);
00324 update();
00325 }
00326
00327 void
00328 CloudEditorWidget::decreasePointSize ()
00329 {
00330 ((MainWindow*) parentWidget()) -> decreaseSpinBoxValue();
00331 point_size_ = ((MainWindow*) parentWidget()) -> getSpinBoxValue();
00332 if (!cloud_ptr_)
00333 return;
00334 cloud_ptr_->setPointSize(point_size_);
00335 update();
00336 }
00337
00338 void
00339 CloudEditorWidget::increaseSelectedPointSize ()
00340 {
00341 ((MainWindow*) parentWidget()) -> increaseSelectedSpinBoxValue();
00342 selected_point_size_ =
00343 ((MainWindow*) parentWidget()) -> getSelectedSpinBoxValue();
00344 if (!cloud_ptr_)
00345 return;
00346 cloud_ptr_->setHighlightPointSize(selected_point_size_);
00347 update();
00348 }
00349
00350 void
00351 CloudEditorWidget::decreaseSelectedPointSize ()
00352 {
00353 ((MainWindow*) parentWidget()) -> decreaseSelectedSpinBoxValue();
00354 selected_point_size_ =
00355 ((MainWindow*) parentWidget()) -> getSelectedSpinBoxValue();
00356 if (!cloud_ptr_)
00357 return;
00358 cloud_ptr_->setHighlightPointSize(selected_point_size_);
00359 update();
00360 }
00361
00362 void
00363 CloudEditorWidget::setPointSize (int size)
00364 {
00365 point_size_ = size;
00366 if (!cloud_ptr_)
00367 return;
00368 cloud_ptr_->setPointSize(size);
00369 update();
00370 }
00371
00372 void
00373 CloudEditorWidget::setSelectedPointSize (int size)
00374 {
00375 selected_point_size_ = size;
00376 if (!cloud_ptr_)
00377 return;
00378 cloud_ptr_ -> setHighlightPointSize(size);
00379 update();
00380 }
00381
00382 void
00383 CloudEditorWidget::colorByRGB ()
00384 {
00385 if(is_colored_)
00386 color_scheme_ = COLOR_BY_RGB;
00387 }
00388
00389 void
00390 CloudEditorWidget::colorByX ()
00391 {
00392 color_scheme_ = COLOR_BY_X;
00393 }
00394
00395 void
00396 CloudEditorWidget::colorByY ()
00397 {
00398 color_scheme_ = COLOR_BY_Y;
00399 }
00400
00401 void
00402 CloudEditorWidget::colorByZ ()
00403 {
00404 color_scheme_ = COLOR_BY_Z;
00405 }
00406
00407 void
00408 CloudEditorWidget::colorByPure ()
00409 {
00410 color_scheme_ = COLOR_BY_PURE;
00411 }
00412
00413 void
00414 CloudEditorWidget::showStat ()
00415 {
00416 stat_dialog_.update();
00417 stat_dialog_.show();
00418 stat_dialog_.raise();
00419 }
00420
00421 void
00422 CloudEditorWidget::initializeGL ()
00423 {
00424 glClearColor(0.0, 0.0, 0.0, 0.0);
00425 glShadeModel(GL_FLAT);
00426 glEnable(GL_DEPTH_TEST);
00427 glDisable(GL_LIGHTING);
00428 glDisable(GL_FOG);
00429 glEnable(GL_POINT_SMOOTH);
00430 glEnable(GL_LINE_SMOOTH);
00431 glEnable( GL_BLEND );
00432 glBlendFunc( GL_ONE, GL_ZERO );
00433 glHint(GL_POINT_SMOOTH_HINT & GL_LINE_SMOOTH_HINT, GL_NICEST);
00434 initTexture();
00435 }
00436
00437 void
00438 CloudEditorWidget::paintGL ()
00439 {
00440 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
00441 glMatrixMode(GL_MODELVIEW);
00442 glLoadIdentity();
00443 if (!cloud_ptr_)
00444 return;
00445 tool_ptr_ -> draw();
00446
00447
00448 if (color_scheme_ == COLOR_BY_RGB)
00449 cloud_ptr_->drawWithRGB();
00450 else if (color_scheme_ == COLOR_BY_PURE)
00451 cloud_ptr_->drawWithPureColor();
00452 else
00453 {
00454
00455
00456 cloud_ptr_ -> setColorRampAxis(Axis(color_scheme_));
00457 cloud_ptr_ -> drawWithTexture();
00458 }
00459
00460
00461 }
00462
00463 void
00464 CloudEditorWidget::resizeGL (int width, int height)
00465 {
00466 glViewport(0, 0, width, height);
00467 cam_aspect_ = double(width) / double(height);
00468 glMatrixMode(GL_PROJECTION);
00469 glLoadIdentity();
00470 gluPerspective(cam_fov_, cam_aspect_, cam_near_, cam_far_);
00471 glMatrixMode(GL_MODELVIEW);
00472 glLoadIdentity();
00473 }
00474
00475 void
00476 CloudEditorWidget::mousePressEvent (QMouseEvent *event)
00477 {
00478 if (!tool_ptr_)
00479 return;
00480 tool_ptr_ -> start(event -> x(), event -> y(),
00481 event -> modifiers(), event -> buttons());
00482 update();
00483 }
00484
00485 void
00486 CloudEditorWidget::mouseMoveEvent (QMouseEvent *event)
00487 {
00488 if (!tool_ptr_)
00489 return;
00490 tool_ptr_ -> update(event -> x(), event -> y(),
00491 event -> modifiers(), event -> buttons());
00492 update();
00493 }
00494
00495 void
00496 CloudEditorWidget::mouseReleaseEvent (QMouseEvent *event)
00497 {
00498 if (!tool_ptr_)
00499 return;
00500 tool_ptr_ -> end(event -> x(), event -> y(),
00501 event -> modifiers(), event -> button());
00502 update();
00503 }
00504
00505 void
00506 CloudEditorWidget::keyPressEvent (QKeyEvent *event)
00507 {
00508 int key = event->key() + static_cast<int>(event->modifiers());
00509 std::map<int, KeyMapFunc>::iterator it = key_map_.find(key);
00510 if (it != key_map_.end())
00511 {
00512 (it->second)(this);
00513 update();
00514 }
00515 }
00516
00517 void
00518 CloudEditorWidget::loadFilePCD(const std::string &filename)
00519 {
00520 PclCloudPtr pcl_cloud_ptr;
00521 Cloud3D tmp;
00522 if (pcl::io::loadPCDFile<Point3D>(filename, tmp) == -1)
00523 throw;
00524 pcl_cloud_ptr = PclCloudPtr(new Cloud3D(tmp));
00525 std::vector<int> index;
00526 pcl::removeNaNFromPointCloud(*pcl_cloud_ptr, *pcl_cloud_ptr, index);
00527 Statistics::clear();
00528 cloud_ptr_ = CloudPtr(new Cloud(*pcl_cloud_ptr, true));
00529 selection_ptr_ = SelectionPtr(new Selection(cloud_ptr_, true));
00530 copy_buffer_ptr_ = CopyBufferPtr(new CopyBuffer(true));
00531 cloud_ptr_->setPointSize(point_size_);
00532 cloud_ptr_->setHighlightPointSize(selected_point_size_);
00533 tool_ptr_ =
00534 boost::shared_ptr<CloudTransformTool>(new CloudTransformTool(cloud_ptr_));
00535
00536 if (isColored(filename))
00537 {
00538 swapRBValues();
00539 color_scheme_ = COLOR_BY_RGB;
00540 is_colored_ = true;
00541 }
00542 else
00543 {
00544 color_scheme_ = COLOR_BY_Z;
00545 is_colored_ = false;
00546 }
00547 }
00548
00549 void
00550 CloudEditorWidget::initFileLoadMap()
00551 {
00552 cloud_load_func_map_.clear();
00553 cloud_load_func_map_["pcd"] = &CloudEditorWidget::loadFilePCD;
00554 }
00555
00556 bool
00557 CloudEditorWidget::isColored (const std::string &fileName) const
00558 {
00559 pcl::PCLPointCloud2 cloud2;
00560 pcl::PCDReader reader;
00561 reader.readHeader(fileName, cloud2);
00562 std::vector< pcl::PCLPointField > fs = cloud2.fields;
00563 for(unsigned int i = 0; i < fs.size(); ++i)
00564 {
00565 std::string name(fs[i].name);
00566 stringToLower(name);
00567 if ((name.compare("rgb") == 0) || (name.compare("rgba") == 0))
00568 return true;
00569 }
00570 return false;
00571 }
00572
00573 void
00574 CloudEditorWidget::swapRBValues ()
00575 {
00576 if (!cloud_ptr_)
00577 return;
00578 for (unsigned int i = 0; i < cloud_ptr_ -> size(); i++)
00579 {
00580 uint8_t cc = (*cloud_ptr_)[i].r;
00581 (*cloud_ptr_)[i].r = (*cloud_ptr_)[i].b;
00582 (*cloud_ptr_)[i].b = cc;
00583 }
00584 }
00585
00586 void
00587 CloudEditorWidget::initKeyMap ()
00588 {
00589 key_map_[Qt::Key_1] = &CloudEditorWidget::colorByPure;
00590 key_map_[Qt::Key_2] = &CloudEditorWidget::colorByX;
00591 key_map_[Qt::Key_3] = &CloudEditorWidget::colorByY;
00592 key_map_[Qt::Key_4] = &CloudEditorWidget::colorByZ;
00593 key_map_[Qt::Key_5] = &CloudEditorWidget::colorByRGB;
00594 key_map_[Qt::Key_C + (int) Qt::ControlModifier] = &CloudEditorWidget::copy;
00595 key_map_[Qt::Key_X + (int) Qt::ControlModifier] = &CloudEditorWidget::cut;
00596 key_map_[Qt::Key_V + (int) Qt::ControlModifier] = &CloudEditorWidget::paste;
00597 key_map_[Qt::Key_S] = &CloudEditorWidget::select2D;
00598 key_map_[Qt::Key_E] = &CloudEditorWidget::select1D;
00599 key_map_[Qt::Key_T] = &CloudEditorWidget::transform;
00600 key_map_[Qt::Key_V] = &CloudEditorWidget::view;
00601 key_map_[Qt::Key_Delete] = &CloudEditorWidget::remove;
00602 key_map_[Qt::Key_Z + (int) Qt::ControlModifier] = &CloudEditorWidget::undo;
00603 key_map_[Qt::Key_Equal] = &CloudEditorWidget::increasePointSize;
00604 key_map_[Qt::Key_Plus] = &CloudEditorWidget::increasePointSize;
00605 key_map_[Qt::Key_Minus] = &CloudEditorWidget::decreasePointSize;
00606 key_map_[Qt::Key_Equal + (int) Qt::ControlModifier] =
00607 &CloudEditorWidget::increaseSelectedPointSize;
00608 key_map_[Qt::Key_Plus + (int) Qt::ControlModifier] =
00609 &CloudEditorWidget::increaseSelectedPointSize;
00610 key_map_[Qt::Key_Minus + (int) Qt::ControlModifier] =
00611 &CloudEditorWidget::decreaseSelectedPointSize;
00612 key_map_[Qt::Key_Escape] = &CloudEditorWidget::cancelSelect;
00613 }
00614
00615 void
00616 CloudEditorWidget::initTexture ()
00617 {
00618 static GLfloat colorWheel[14][3] =
00619 {
00620 { 0.0f, 0.0f, 1.0000f},
00621 { 0.0f, 0.2500f, 1.0000f},
00622 { 0.0f, 0.5000f, 1.0000f},
00623 { 0.0f, 0.7500f, 1.0000f},
00624 { 0.0f, 1.0000f, 1.0000f},
00625 { 0.2500f, 1.0000f, 1.0000f},
00626 { 0.5000f, 1.0000f, 0.7500f},
00627 { 0.7500f, 1.0000f, 0.5000f},
00628 { 1.0000f, 1.0000f, 0.2500f},
00629 { 1.0000f, 1.0000f, 0.0f},
00630 { 1.0000f, 0.7500f, 0.0f},
00631 { 1.0000f, 0.5000f, 0.0f},
00632 { 1.0000f, 0.2500f, 0.0f},
00633 { 1.0000f, 0.0f, 0.0f},
00634 };
00635 GLuint textures;
00636 glGenTextures(1,&textures);
00637 glBindTexture(GL_TEXTURE_1D,textures);
00638 glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);
00639 glTexParameteri(GL_TEXTURE_1D, GL_TEXTURE_WRAP_S, GL_REPEAT);
00640 glTexParameteri(GL_TEXTURE_1D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
00641 glTexParameteri(GL_TEXTURE_1D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
00642 glTexImage1D(GL_TEXTURE_1D, 0, GL_RGB, 14, 0, GL_RGB , GL_FLOAT, colorWheel);
00643 }