diff --git a/corelib/include/rtabmap/core/CameraRGBD.h b/corelib/include/rtabmap/core/CameraRGBD.h index 82adeb30..0cbd8710 100644 --- a/corelib/include/rtabmap/core/CameraRGBD.h +++ b/corelib/include/rtabmap/core/CameraRGBD.h @@ -69,10 +69,12 @@ public: //getters float getImageRate() const {return _imageRate;} const Transform & getLocalTransform() const {return _localTransform;} + float getFocalLength() const {return _focalLength;} //setters void setImageRate(float imageRate) {_imageRate = imageRate;} void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;} + void setFocalLength(float focalLength) {_focalLength = focalLength;} protected: /** @@ -81,7 +83,8 @@ protected: * @param imageRate : image/second , 0 for fast as the camera can */ CameraRGBD(float imageRate = 0, - const Transform & localTransform = Transform::getIdentity()); + const Transform & localTransform = Transform::getIdentity(), + float focalLength = 0.0f); virtual void captureImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant) = 0; @@ -89,6 +92,7 @@ private: float _imageRate; Transform _localTransform; UTimer * _frameRateTimer; + float _focalLength; }; ///////////////////////// @@ -104,7 +108,8 @@ public: // default local transform z in, x right, y down)); CameraOpenni(const std::string & deviceId="", float imageRate = 0, - const Transform & localTransform = Transform::getIdentity()); + const Transform & localTransform = Transform::getIdentity(), + float focalLength = 0.0f); virtual ~CameraOpenni(); void image_cb ( @@ -141,7 +146,8 @@ public: public: CameraOpenNICV(bool asus = false, float imageRate = 0, - const Transform & localTransform = Transform::getIdentity()); + const Transform & localTransform = Transform::getIdentity(), + float focalLength = 0.0f); virtual ~CameraOpenNICV(); virtual bool init(); @@ -167,7 +173,8 @@ public: public: CameraOpenNI2(float imageRate = 0, - const Transform & localTransform = Transform::getIdentity()); + const Transform & localTransform = Transform::getIdentity(), + float focalLength = 0.0f); virtual ~CameraOpenNI2(); virtual bool init(); @@ -198,7 +205,8 @@ public: // default local transform z in, x right, y down)); CameraFreenect(int deviceId= 0, float imageRate=0.0f, - const Transform & localTransform = Transform::getIdentity()); + const Transform & localTransform = Transform::getIdentity(), + float focalLength = 0.0f); virtual ~CameraFreenect(); bool init(); diff --git a/corelib/src/CameraRGBD.cpp b/corelib/src/CameraRGBD.cpp index 09bdbeed..11d9f2fb 100644 --- a/corelib/src/CameraRGBD.cpp +++ b/corelib/src/CameraRGBD.cpp @@ -50,10 +50,11 @@ namespace rtabmap { -CameraRGBD::CameraRGBD(float imageRate, const Transform & localTransform) : +CameraRGBD::CameraRGBD(float imageRate, const Transform & localTransform, float focalLength) : _imageRate(imageRate), _localTransform(localTransform), - _frameRateTimer(new UTimer()) + _frameRateTimer(new UTimer()), + _focalLength(focalLength) { } @@ -89,14 +90,18 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & depthConstant UTimer timer; this->captureImage(rgb, depth, depthConstant); + if(_focalLength) + { + depthConstant = 1.0f/_focalLength; // override if set + } UDEBUG("Time capturing image = %fs", timer.ticks()); } ///////////////////////// // CameraOpenNIPCL ///////////////////////// -CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform) : - CameraRGBD(imageRate, localTransform), +CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform, float focalLength) : + CameraRGBD(imageRate, localTransform, focalLength), interface_(0), deviceId_(deviceId), depthConstant_(0.0f) @@ -205,8 +210,8 @@ bool CameraOpenNICV::available() return cv::getBuildInformation().find("OpenNI: YES") != std::string::npos; } -CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform) : - CameraRGBD(imageRate, localTransform), +CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform, float focalLength) : + CameraRGBD(imageRate, localTransform, focalLength), _asus(asus), _depthFocal(0.0f) { @@ -299,8 +304,8 @@ bool CameraOpenNI2::available() #endif } -CameraOpenNI2::CameraOpenNI2(float imageRate, const rtabmap::Transform & localTransform) : - CameraRGBD(imageRate, localTransform), +CameraOpenNI2::CameraOpenNI2(float imageRate, const rtabmap::Transform & localTransform, float focalLength) : + CameraRGBD(imageRate, localTransform, focalLength), #ifdef WITH_OPENNI2 _device(new openni::Device()), _color(new openni::VideoStream()), @@ -685,8 +690,8 @@ bool CameraFreenect::available() #endif } -CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform & localTransform) : - CameraRGBD(imageRate, localTransform), +CameraFreenect::CameraFreenect(int deviceId, float imageRate, const Transform & localTransform, float focalLength) : + CameraRGBD(imageRate, localTransform, focalLength), deviceId_(deviceId), ctx_(0), freenectDevice_(0) diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index e07ff699..c95c110c 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -195,8 +195,8 @@ private: float voxelSize, int decimation, float maxDepth) const; - std::map::Ptr > createPointClouds(const std::map & poses) const; - std::map createMeshes(const std::map & poses) const; + std::map::Ptr > createPointClouds(const std::map & poses, bool applyMLS) const; + std::map createMeshes(const std::map & poses, bool applyMLS) const; void savePointClouds(const std::map::Ptr> & clouds); void saveMeshes(const std::map & meshes); diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 15247d61..2031f2c0 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -159,6 +159,7 @@ public: Src getSourceRGBD() const; // Openni group QString getSourceOpenniDevice() const; //Openni group Transform getSourceOpenniLocalTransform() const; //Openni group + float getSourceOpenniFocalLength() const; // Openni group int getIgnoredDCComponents() const; diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index f9f4bebf..3315c210 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -1962,20 +1962,23 @@ void MainWindow::startDetection() camera = new CameraOpenni( _preferencesDialog->getSourceOpenniDevice().toStdString(), _preferencesDialog->getGeneralInputRate(), - _preferencesDialog->getSourceOpenniLocalTransform()); + _preferencesDialog->getSourceOpenniLocalTransform(), + _preferencesDialog->getSourceOpenniFocalLength()); } else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2) { camera = new CameraOpenNI2( _preferencesDialog->getGeneralInputRate(), - _preferencesDialog->getSourceOpenniLocalTransform()); + _preferencesDialog->getSourceOpenniLocalTransform(), + _preferencesDialog->getSourceOpenniFocalLength()); } else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect) { camera = new CameraFreenect( _preferencesDialog->getSourceOpenniDevice().isEmpty()?0:atoi(_preferencesDialog->getSourceOpenniDevice().toStdString().c_str()), _preferencesDialog->getGeneralInputRate(), - _preferencesDialog->getSourceOpenniLocalTransform()); + _preferencesDialog->getSourceOpenniLocalTransform(), + _preferencesDialog->getSourceOpenniFocalLength()); } else if(_preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV || _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS) @@ -1983,7 +1986,8 @@ void MainWindow::startDetection() camera = new CameraOpenNICV( _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI_CV_ASUS, _preferencesDialog->getGeneralInputRate(), - _preferencesDialog->getSourceOpenniLocalTransform()); + _preferencesDialog->getSourceOpenniLocalTransform(), + _preferencesDialog->getSourceOpenniFocalLength()); } else { @@ -2804,8 +2808,14 @@ void MainWindow::savePointClouds() { int button = QMessageBox::question(this, tr("One or multiple files?"), - tr("Merge all clouds together?"), - QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); + tr("Merge all clouds together?" + "\n\nNote that all saving parameters [decimation=%1, voxel=%2m, max depth=%3m] can be found in " + "Preferences -> GUI -> 3D Rendering under Saving column.") + .arg(_preferencesDialog->getCloudDecimation(2)) + .arg(_preferencesDialog->getCloudVoxelSize(2)) + .arg(_preferencesDialog->getCloudMaxDepth(2)), + QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel, + QMessageBox::Yes); if(button == QMessageBox::Yes || button == QMessageBox::No) { @@ -2820,22 +2830,45 @@ void MainWindow::savePointClouds() if(button == QMessageBox::Yes) { pcl::PointCloud::Ptr cloud = this->createAssembledCloud(poses); - if(_preferencesDialog->getMeshSmoothing(1)) + button = QMessageBox::question( + this, + tr("Smoothing..."), + tr("Would you want to smooth the surface using Moving " + "Least Squares algorithm? (The cloud has %1 points, this " + "may take a while to process!)").arg(cloud->size()), + QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel, + QMessageBox::No); + if(button == QMessageBox::Yes || button == QMessageBox::No) { - pcl::PointCloud::Ptr cloudWithNormals; - _initProgressDialog->appendText(tr("Smoothing the surface using Moving Least Squares algorithm...")); - _initProgressDialog->incrementStep(); - QApplication::processEvents(); + if(button == QMessageBox::Yes) + { + pcl::PointCloud::Ptr cloudWithNormals; + _initProgressDialog->appendText(tr("Smoothing the surface using Moving Least Squares (MLS) algorithm... " + "[search radius=%1m]").arg(_preferencesDialog->getMeshSmoothingRadius(1))); + _initProgressDialog->incrementStep(); + QApplication::processEvents(); - cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1)); - cloud->clear(); - pcl::copyPointCloud(*cloudWithNormals, *cloud); + cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1)); + cloud->clear(); + pcl::copyPointCloud(*cloudWithNormals, *cloud); + } + clouds.insert(std::make_pair(0, cloud)); } - clouds.insert(std::make_pair(0, cloud)); } else { - clouds = this->createPointClouds(poses); + button = QMessageBox::question( + this, + tr("Smoothing..."), + tr("Would you want to smooth the surface using Moving " + "Least Squares algorithm? (This " + "may take a while to process!)"), + QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel, + QMessageBox::No); + if(button == QMessageBox::Yes || button == QMessageBox::No) + { + clouds = this->createPointClouds(poses, button == QMessageBox::Yes); + } } savePointClouds(clouds); _initProgressDialog->setValue(_initProgressDialog->maximumSteps()); @@ -2848,8 +2881,14 @@ void MainWindow::saveMeshes() tr("One or multiple files?"), tr("Merge all clouds together before surface reconstruction?\n" " Yes: Output a single mesh for the merged clouds.\n" - " No: Output a mesh for each cloud."), - QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); + " No: Output a mesh for each cloud." + "\n\nNote that all saving parameters [decimation=%1, voxel=%2m, max depth=%3m] can be found in " + "Preferences -> GUI -> 3D Rendering under Saving column.") + .arg(_preferencesDialog->getCloudDecimation(2)) + .arg(_preferencesDialog->getCloudVoxelSize(2)) + .arg(_preferencesDialog->getCloudMaxDepth(2)), + QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel, + QMessageBox::Yes); if(button == QMessageBox::Yes || button == QMessageBox::No) { @@ -2869,33 +2908,57 @@ void MainWindow::saveMeshes() QApplication::processEvents(); pcl::PointCloud::Ptr cloudWithNormals; - if(_preferencesDialog->getMeshSmoothing(1)) + button = QMessageBox::question( + this, + tr("Smoothing..."), + tr("Would you want to smooth the surface using Moving " + "Least Squares algorithm? (The cloud has %1 points, this " + "may take a while to process!)").arg(cloud->size()), + QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel, + QMessageBox::No); + if(button == QMessageBox::Yes || button == QMessageBox::No) { - _initProgressDialog->appendText(tr("Smoothing the surface using Moving Least Squares algorithm...")); + if(button == QMessageBox::Yes) + { + _initProgressDialog->appendText(tr("Smoothing the surface using Moving Least Squares (MLS) algorithm... " + "[search radius=%1m]").arg(_preferencesDialog->getMeshSmoothingRadius(1))); + _initProgressDialog->incrementStep(); + QApplication::processEvents(); + + cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1)); + } + else + { + _initProgressDialog->appendText(tr("Computing surface normals (without smoothing)... " + "[K neighbors=%1]").arg(_preferencesDialog->getMeshNormalKSearch(1))); + _initProgressDialog->incrementStep(); + QApplication::processEvents(); + + cloudWithNormals = util3d::computeNormals(cloud, _preferencesDialog->getMeshNormalKSearch(1)); + } + + _initProgressDialog->appendText(tr("Greedy projection triangulation... [radius=%1m]").arg(_preferencesDialog->getMeshGP3Radius(1))); _initProgressDialog->incrementStep(); QApplication::processEvents(); - cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1)); + pcl::PolygonMesh::Ptr mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius(1)); + meshes.insert(std::make_pair(0, mesh)); } - else - { - _initProgressDialog->appendText(tr("Computing surface normals (without smoothing)...")); - _initProgressDialog->incrementStep(); - QApplication::processEvents(); - - cloudWithNormals = util3d::computeNormals(cloud, _preferencesDialog->getMeshNormalKSearch(1)); - } - - _initProgressDialog->appendText(tr("Greedy projection triangulation...")); - _initProgressDialog->incrementStep(); - QApplication::processEvents(); - - pcl::PolygonMesh::Ptr mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius(1)); - meshes.insert(std::make_pair(0, mesh)); } else { - meshes = this->createMeshes(poses); + button = QMessageBox::question( + this, + tr("Smoothing..."), + tr("Would you want to smooth the surface using Moving " + "Least Squares algorithm? (This " + "may take a while to process!)"), + QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel, + QMessageBox::No); + if(button == QMessageBox::Yes || button == QMessageBox::No) + { + meshes = this->createMeshes(poses, button == QMessageBox::Yes); + } } saveMeshes(meshes); _initProgressDialog->setValue(_initProgressDialog->maximumSteps()); @@ -2908,8 +2971,14 @@ void MainWindow::viewPointClouds() { int button = QMessageBox::question(this, tr("One or multiple clouds?"), - tr("Merge all clouds together?"), - QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); + tr("Merge all clouds together?" + "\n\nNote that all saving parameters [decimation=%1, voxel=%2m, max depth=%3m] can be found in " + "Preferences -> GUI -> 3D Rendering under High res view column.") + .arg(_preferencesDialog->getCloudDecimation(2)) + .arg(_preferencesDialog->getCloudVoxelSize(2)) + .arg(_preferencesDialog->getCloudMaxDepth(2)), + QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel, + QMessageBox::Yes); if(button == QMessageBox::Yes || button == QMessageBox::No) { @@ -2925,23 +2994,46 @@ void MainWindow::viewPointClouds() { pcl::PointCloud::Ptr cloud = this->createAssembledCloud(poses); - if(_preferencesDialog->getMeshSmoothing(1)) + button = QMessageBox::question( + this, + tr("Smoothing..."), + tr("Would you want to smooth the surface using Moving " + "Least Squares algorithm? (The cloud has %1 points, this " + "may take a while to process!)").arg(cloud->size()), + QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel, + QMessageBox::No); + if(button == QMessageBox::Yes || button == QMessageBox::No) { - pcl::PointCloud::Ptr cloudWithNormals; - _initProgressDialog->appendText(tr("Smoothing the surface using Moving Least Squares algorithm...")); - _initProgressDialog->incrementStep(); - QApplication::processEvents(); + if(button == QMessageBox::Yes) + { + pcl::PointCloud::Ptr cloudWithNormals; + _initProgressDialog->appendText(tr("Smoothing the surface using Moving Least Squares (MLS) algorithm... " + "[search radius=%1m]").arg(_preferencesDialog->getMeshSmoothingRadius(1))); + _initProgressDialog->incrementStep(); + QApplication::processEvents(); - cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1)); - cloud->clear(); - pcl::copyPointCloud(*cloudWithNormals, *cloud); + cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1)); + cloud->clear(); + pcl::copyPointCloud(*cloudWithNormals, *cloud); + } + + clouds.insert(std::make_pair(0, cloud)); } - - clouds.insert(std::make_pair(0, cloud)); } else { - clouds = this->createPointClouds(poses); + button = QMessageBox::question( + this, + tr("Smoothing..."), + tr("Would you want to smooth the surface using Moving " + "Least Squares algorithm? (This " + "may take a while to process!)"), + QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel, + QMessageBox::No); + if(button == QMessageBox::Yes || button == QMessageBox::No) + { + clouds = this->createPointClouds(poses, button == QMessageBox::Yes); + } } if(clouds.size()) @@ -2972,10 +3064,6 @@ void MainWindow::viewPointClouds() _initProgressDialog->appendText(tr("Viewing the cloud %1 (%2 points)... done.").arg(iter->first).arg(iter->second->size())); } } - else - { - QMessageBox::warning(this, tr("Viewing clouds failed!"), tr("Clouds are empty...")); - } _initProgressDialog->setValue(_initProgressDialog->maximumSteps()); } @@ -2987,8 +3075,14 @@ void MainWindow::viewMeshes() tr("One or multiple meshes?"), tr("Merge all clouds together before surface reconstruction?\n" " Yes: Output a single mesh for the merged clouds.\n" - " No: Output a mesh for each cloud."), - QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); + " No: Output a mesh for each cloud." + "\n\nNote that all saving parameters [decimation=%1, voxel=%2m, max depth=%3m] can be found in " + "Preferences -> GUI -> 3D Rendering under High res view column.") + .arg(_preferencesDialog->getCloudDecimation(2)) + .arg(_preferencesDialog->getCloudVoxelSize(2)) + .arg(_preferencesDialog->getCloudMaxDepth(2)), + QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel, + QMessageBox::Yes); if(button == QMessageBox::Yes || button == QMessageBox::No) { @@ -3008,33 +3102,57 @@ void MainWindow::viewMeshes() QApplication::processEvents(); pcl::PointCloud::Ptr cloudWithNormals; - if(_preferencesDialog->getMeshSmoothing(1)) + button = QMessageBox::question( + this, + tr("Smoothing..."), + tr("Would you want to smooth the surface using Moving " + "Least Squares algorithm? (The cloud has %1 points, this " + "may take a while to process!)").arg(cloud->size()), + QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel, + QMessageBox::No); + if(button == QMessageBox::Yes || button == QMessageBox::No) { - _initProgressDialog->appendText(tr("Smoothing the surface using Moving Least Squares algorithm...")); + if(button == QMessageBox::Yes) + { + _initProgressDialog->appendText(tr("Smoothing the surface using Moving Least Squares (MLS) algorithm... " + "[search radius=%1m]").arg(_preferencesDialog->getMeshSmoothingRadius(1))); + _initProgressDialog->incrementStep(); + QApplication::processEvents(); + + cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1)); + } + else + { + _initProgressDialog->appendText(tr("Computing surface normals (without smoothing)... " + "[K neighbors=%1]").arg(_preferencesDialog->getMeshNormalKSearch(1))); + _initProgressDialog->incrementStep(); + QApplication::processEvents(); + + cloudWithNormals = util3d::computeNormals(cloud, _preferencesDialog->getMeshNormalKSearch(1)); + } + + _initProgressDialog->appendText(tr("Greedy projection triangulation... [radius=%1m]").arg(_preferencesDialog->getMeshGP3Radius(1))); _initProgressDialog->incrementStep(); QApplication::processEvents(); - cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1)); + pcl::PolygonMesh::Ptr mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius(1)); + meshes.insert(std::make_pair(0, mesh)); } - else - { - _initProgressDialog->appendText(tr("Computing surface normals (without smoothing)...")); - _initProgressDialog->incrementStep(); - QApplication::processEvents(); - - cloudWithNormals = util3d::computeNormals(cloud, _preferencesDialog->getMeshNormalKSearch(1)); - } - - _initProgressDialog->appendText(tr("Greedy projection triangulation...")); - _initProgressDialog->incrementStep(); - QApplication::processEvents(); - - pcl::PolygonMesh::Ptr mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius(1)); - meshes.insert(std::make_pair(0, mesh)); } else { - meshes = this->createMeshes(poses); + button = QMessageBox::question( + this, + tr("Smoothing..."), + tr("Would you want to smooth the surface using Moving " + "Least Squares algorithm? (This " + "may take a while to process!)"), + QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel, + QMessageBox::No); + if(button == QMessageBox::Yes || button == QMessageBox::No) + { + meshes = this->createMeshes(poses, button == QMessageBox::Yes); + } } if(meshes.size()) @@ -3067,10 +3185,6 @@ void MainWindow::viewMeshes() QApplication::processEvents(); } } - else - { - QMessageBox::warning(this, tr("Viewing meshes failed!"), tr("Meshes are empty...")); - } _initProgressDialog->setValue(_initProgressDialog->maximumSteps()); } @@ -3485,7 +3599,9 @@ pcl::PointCloud::Ptr MainWindow::createAssembledCloud(const st return assembledCloud; } -std::map::Ptr > MainWindow::createPointClouds(const std::map & poses) const +std::map::Ptr > MainWindow::createPointClouds( + const std::map & poses, + bool applyMLS) const { std::map::Ptr> clouds; int i=0; @@ -3515,7 +3631,7 @@ std::map::Ptr > MainWindow::createPointCl if(cloud->size()) { - if(_preferencesDialog->getMeshSmoothing(1)) + if(applyMLS) { pcl::PointCloud::Ptr cloudWithNormals; cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1)); @@ -3552,7 +3668,7 @@ std::map::Ptr > MainWindow::createPointCl return clouds; } -std::map MainWindow::createMeshes(const std::map & poses) const +std::map MainWindow::createMeshes(const std::map & poses, bool applyMLS) const { std::map meshes; int i=0; @@ -3584,7 +3700,7 @@ std::map MainWindow::createMeshes(const std::mapsize()) { pcl::PointCloud::Ptr cloudWithNormals; - if(_preferencesDialog->getMeshSmoothing(1)) + if(applyMLS) { cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1)); } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 34da904a..c4484f09 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -174,7 +174,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _3dRenderingSmoothing.resize(2); _3dRenderingSmoothing[0] = _ui->checkBox_mls; - _3dRenderingSmoothing[1] = _ui->checkBox_mls_save; _3dRenderingSmoothingRadius.resize(2); _3dRenderingSmoothingRadius[0] = _ui->doubleSpinBox_mlsRadius; @@ -197,13 +196,13 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_3dRenderingNormalKSearch[i], SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_3dRenderingGP3Radius[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); - connect(_3dRenderingSmoothing[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_3dRenderingSmoothingRadius[i], SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); } if(i<1) { connect(_3dRenderingMeshing[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_3dRenderingSmoothing[i], SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); } } @@ -257,6 +256,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->radioButton_openni2->setEnabled(CameraOpenNI2::available()); connect(_ui->lineEdit_openniDevice, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->lineEdit_openniLocalTransform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->doubleSpinBox_openniFocal, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel())); + //Rtabmap basic connect(_ui->general_doubleSpinBox_timeThr, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_timeThr_2, SLOT(setValue(double))); @@ -762,13 +763,13 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _3dRenderingNormalKSearch[i]->setValue(20); _3dRenderingGP3Radius[i]->setValue(0.04); - _3dRenderingSmoothing[i]->setChecked(i==0?false:true); _3dRenderingSmoothingRadius[i]->setValue(0.04); } if(i<1) { _3dRenderingMeshing[i]->setChecked(false); + _3dRenderingSmoothing[i]->setChecked(false); } } @@ -806,6 +807,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->radioButton_opennicvasus->setChecked(false); _ui->lineEdit_openniDevice->setText(""); _ui->lineEdit_openniLocalTransform->setText("0 0 0 -PI_2 0 -PI_2"); + _ui->doubleSpinBox_openniFocal->setValue(0.0); } else if(groupBox->objectName() == _ui->groupBox_rtabmap_basic0->objectName()) { @@ -995,13 +997,13 @@ void PreferencesDialog::readGuiSettings(const QString & filePath) _3dRenderingNormalKSearch[i]->setValue(settings.value(tr("meshNormalKSearch%1").arg(i), _3dRenderingNormalKSearch[i]->value()).toInt()); _3dRenderingGP3Radius[i]->setValue(settings.value(tr("meshGP3Radius%1").arg(i), _3dRenderingGP3Radius[i]->value()).toDouble()); - _3dRenderingSmoothing[i]->setChecked(settings.value(tr("meshSmoothing%1").arg(i), _3dRenderingSmoothing[i]->isChecked()).toBool()); _3dRenderingSmoothingRadius[i]->setValue(settings.value(tr("meshSmoothingRadius%1").arg(i), _3dRenderingSmoothingRadius[i]->value()).toDouble()); } if(i<1) { _3dRenderingMeshing[i]->setChecked(settings.value(tr("meshing%1").arg(i), _3dRenderingMeshing[i]->isChecked()).toBool()); + _3dRenderingSmoothing[i]->setChecked(settings.value(tr("meshSmoothing%1").arg(i), _3dRenderingSmoothing[i]->isChecked()).toBool()); } } @@ -1061,6 +1063,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) _ui->radioButton_opennicvasus->setChecked(settings.value("openniCvAsusType", _ui->radioButton_opennicvasus->isChecked()).toBool()); _ui->lineEdit_openniDevice->setText(settings.value("device",_ui->lineEdit_openniDevice->text()).toString()); _ui->lineEdit_openniLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_openniLocalTransform->text()).toString()); + _ui->doubleSpinBox_openniFocal->setValue(settings.value("focalLength", _ui->doubleSpinBox_openniFocal->value()).toDouble()); settings.endGroup(); // Openni } @@ -1205,13 +1208,13 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) settings.setValue(tr("meshNormalKSearch%1").arg(i), _3dRenderingNormalKSearch[i]->value()); settings.setValue(tr("meshGP3Radius%1").arg(i), _3dRenderingGP3Radius[i]->value()); - settings.setValue(tr("meshSmoothing%1").arg(i), _3dRenderingSmoothing[i]->isChecked()); settings.setValue(tr("meshSmoothingRadius%1").arg(i), _3dRenderingSmoothingRadius[i]->value()); } if(i<1) { settings.setValue(tr("meshing%1").arg(i), _3dRenderingMeshing[i]->isChecked()); + settings.setValue(tr("meshSmoothing%1").arg(i), _3dRenderingSmoothing[i]->isChecked()); } } settings.setValue("cloudFiltering", _ui->groupBox_poseFiltering->isChecked()); @@ -1270,6 +1273,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) settings.setValue("openniCvAsusType", _ui->radioButton_opennicvasus->isChecked()); settings.setValue("device", _ui->lineEdit_openniDevice->text()); settings.setValue("localTransform", _ui->lineEdit_openniLocalTransform->text()); + settings.setValue("focalLength", _ui->doubleSpinBox_openniFocal->value()); settings.endGroup(); } @@ -2432,7 +2436,7 @@ bool PreferencesDialog::isCloudsShown(int index) const } bool PreferencesDialog::isCloudMeshing(int index) const { - UASSERT(index >= 0 && index <= 1); + UASSERT(index == 0); return _3dRenderingMeshing[index]->isChecked(); } double PreferencesDialog::getCloudVoxelSize(int index) const @@ -2488,7 +2492,7 @@ double PreferencesDialog::getMeshGP3Radius(int index) const } bool PreferencesDialog::getMeshSmoothing(int index) const { - UASSERT(index >= 0 && index <= 1); + UASSERT(index == 0); return _3dRenderingSmoothing[index]->isChecked(); } double PreferencesDialog::getMeshSmoothingRadius(int index) const @@ -2636,6 +2640,11 @@ Transform PreferencesDialog::getSourceOpenniLocalTransform() const return t; } +float PreferencesDialog::getSourceOpenniFocalLength() const +{ + return _ui->doubleSpinBox_openniFocal->value(); +} + bool PreferencesDialog::isStatisticsPublished() const { return _ui->general_checkBox_publishStats->isChecked(); @@ -2768,13 +2777,15 @@ void PreferencesDialog::testOdometry(int type) camera = new CameraOpenni( this->getSourceOpenniDevice().toStdString(), this->getGeneralInputRate(), - this->getSourceOpenniLocalTransform()); + this->getSourceOpenniLocalTransform(), + this->getSourceOpenniFocalLength()); } else if(this->getSourceRGBD() == kSrcOpenNI2) { camera = new CameraOpenNI2( this->getGeneralInputRate(), - this->getSourceOpenniLocalTransform()); + this->getSourceOpenniLocalTransform(), + this->getSourceOpenniFocalLength()); } else if(this->getSourceRGBD() == kSrcFreenect) @@ -2782,14 +2793,16 @@ void PreferencesDialog::testOdometry(int type) camera = new CameraFreenect( this->getSourceOpenniDevice().isEmpty()?0:atoi(this->getSourceOpenniDevice().toStdString().c_str()), this->getGeneralInputRate(), - this->getSourceOpenniLocalTransform()); + this->getSourceOpenniLocalTransform(), + this->getSourceOpenniFocalLength()); } else if(this->getSourceRGBD() == kSrcOpenNI_CV || this->getSourceRGBD() == kSrcOpenNI_CV_ASUS) { camera = new CameraOpenNICV( this->getSourceRGBD() == kSrcOpenNI_CV_ASUS, this->getGeneralInputRate(), - this->getSourceOpenniLocalTransform()); + this->getSourceOpenniLocalTransform(), + this->getSourceOpenniFocalLength()); } else { diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 6f5e575f..83cdd4b4 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - -457 + -313 709 - 1140 + 853 @@ -86,7 +86,7 @@ QFrame::Raised - 17 + 1 @@ -1019,16 +1019,6 @@ High-res view - - - - - - - true - - - @@ -1686,6 +1676,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki OpenNI-PCL + + true + @@ -1770,6 +1763,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + 1000.000000000000000 + + + + + + + Intrinsic focal length (fx=fy, K[4]). Set 0 to use the default from the camera. Examples: ~525 for Kinect, ~545 for Xtion Pro Live. + + + true + + +