diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index fe8b32e0..aad2b3bf 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -284,6 +284,7 @@ private: double _lastProcessTime; bool _someNodesHaveBeenTransferred; float _distanceTravelled; + bool _optimizeFromGraphEndChanged; // Abstract classes containing all loop closure // strategies for a type of signature or configuration. diff --git a/corelib/include/rtabmap/core/camera/CameraMyntEye.h b/corelib/include/rtabmap/core/camera/CameraMyntEye.h index 730825b9..26b69e02 100644 --- a/corelib/include/rtabmap/core/camera/CameraMyntEye.h +++ b/corelib/include/rtabmap/core/camera/CameraMyntEye.h @@ -61,6 +61,7 @@ public: virtual bool odomProvided() const { return false; } void publishInterIMU(bool enabled); + void setAutoExposure(bool enabled, int manualGain=24, int manualBrightness=120, int manualConstrast=116); protected: /** @@ -80,6 +81,10 @@ private: std::string deviceName_; bool apiRectification_; bool apiDepth_; + bool autoExposure_; + int gain_; + int brightness_; + int contrast_; USemaphore dataReady_; UMutex dataMutex_; cv::Mat leftFrameBuffer_; @@ -94,7 +99,7 @@ private: double softTimeBegin_; std::uint64_t hardTimeBegin_; - std::uint64_t unitHardTime; + std::uint64_t unitHardTime_; std::vector lastHardTimes_; std::vector acc_; #endif diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index f2a026ea..31ed2199 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -133,6 +133,7 @@ Rtabmap::Rtabmap() : _lastProcessTime(0.0), _someNodesHaveBeenTransferred(false), _distanceTravelled(0.0f), + _optimizeFromGraphEndChanged(false), _epipolarGeometry(0), _bayesFilter(0), _graphOptimizer(0), @@ -375,6 +376,7 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath) _odomCachePoses.clear(); _odomCacheConstraints.clear(); _distanceTravelled = 0.0f; + _optimizeFromGraphEndChanged = false; this->clearPath(0); _gpsGeocentricCache.clear(); _currentSessionHasGPS = false; @@ -478,7 +480,12 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) _proximityAngle *= M_PI/180.0f; } Parameters::parse(parameters, Parameters::kRGBDProximityOdomGuess(), _proximityOdomGuess); + bool optimizeFromGraphEndPrevious = _optimizeFromGraphEnd; Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd); + if(optimizeFromGraphEndPrevious != _optimizeFromGraphEnd) + { + _optimizeFromGraphEndChanged = true; + } Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxError); if(_optimizationMaxError > 0.0 && _optimizationMaxError < 1.0) { @@ -887,6 +894,7 @@ void Rtabmap::resetMemory() _odomCachePoses.clear(); _odomCacheConstraints.clear(); _distanceTravelled = 0.0f; + _optimizeFromGraphEndChanged = false; this->clearPath(0); if(_memory) @@ -2874,23 +2882,12 @@ bool Rtabmap::process( std::map poses = _optimizedPoses; // if _optimizeFromGraphEnd parameter just changed state, don't use optimized poses as guess - float normMapCorrection = _mapCorrection.getNormSquared(); // use distance for identity detection - if((normMapCorrection > 0.001f && _optimizeFromGraphEnd) || - (normMapCorrection < 0.001f && !_optimizeFromGraphEnd)) + if(_optimizeFromGraphEndChanged) { - for(std::multimap::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter) - { - if( iter->second.type() != Link::kNeighbor && - iter->second.type() != Link::kVirtualClosure && - iter->second.type() != Link::kLandmark && - iter->second.type() != Link::kGravity && - iter->second.type() != Link::kPosePrior) - { - UWARN("Optimization: clearing guess poses as %s may have changed state, now %s (normMapCorrection=%f)", Parameters::kRGBDOptimizeFromGraphEnd().c_str(), _optimizeFromGraphEnd?"true":"false", normMapCorrection); - poses.clear(); - break; - } - } + UWARN("Optimization: clearing guess poses as %s has changed state, now %s", + Parameters::kRGBDOptimizeFromGraphEnd().c_str(), _optimizeFromGraphEnd?"true":"false"); + poses.clear(); + _optimizeFromGraphEndChanged = false; } std::multimap constraints; diff --git a/corelib/src/camera/CameraMyntEye.cpp b/corelib/src/camera/CameraMyntEye.cpp index f3dc2c30..954ab496 100644 --- a/corelib/src/camera/CameraMyntEye.cpp +++ b/corelib/src/camera/CameraMyntEye.cpp @@ -45,13 +45,17 @@ CameraMyntEye::CameraMyntEye(const std::string & device, bool apiRectification, deviceName_(device), apiRectification_(apiRectification), apiDepth_(apiDepth), + autoExposure_(true), + gain_(24), + brightness_(120), + contrast_(116), dataReady_(0), lastFramesStamp_(0.0), stamp_(0), publishInterIMU_(false), softTimeBegin_(0.0), hardTimeBegin_(0), - unitHardTime(std::numeric_limits::max()*10) + unitHardTime_(std::numeric_limits::max()*10) #endif { #ifdef RTABMAP_MYNTEYE @@ -133,13 +137,13 @@ inline bool is_overflow(std::uint64_t now, std::uint64_t pre, std::uint64_t unit double CameraMyntEye::checkUpTimeStamp(std::uint64_t _hard_time, std::uint8_t stream) { UASSERT(stream < (std::uint8_t)mynteye::Stream::LAST+1); - if (is_overflow(_hard_time, lastHardTimes_[stream], unitHardTime)) { + if (is_overflow(_hard_time, lastHardTimes_[stream], unitHardTime_)) { acc_[stream]++; } lastHardTimes_[stream] = _hard_time; - return hardTimeToSoftTime(acc_[stream] * unitHardTime + _hard_time); + return hardTimeToSoftTime(acc_[stream] * unitHardTime_ + _hard_time); } #endif @@ -150,6 +154,16 @@ void CameraMyntEye::publishInterIMU(bool enabled) #endif } +void CameraMyntEye::setAutoExposure(bool enabled, int manualGain, int manualBrightness, int manualConstrast) +{ +#ifdef RTABMAP_MYNTEYE + autoExposure_ = enabled; + gain_ = manualGain; + brightness_ = manualBrightness; + contrast_ = manualConstrast; +#endif +} + bool CameraMyntEye::init(const std::string & calibrationFolder, const std::string & cameraName) { #ifdef RTABMAP_MYNTEYE @@ -430,6 +444,14 @@ bool CameraMyntEye::init(const std::string & calibrationFolder, const std::strin }); + api_->SetOptionValue(mynteye::Option::EXPOSURE_MODE, autoExposure_?0:1); + if(!autoExposure_) + { + api_->SetOptionValue(mynteye::Option::GAIN, gain_); + api_->SetOptionValue(mynteye::Option::BRIGHTNESS, brightness_); + api_->SetOptionValue(mynteye::Option::CONTRAST, contrast_); + } + api_->Start(mynteye::Source::ALL); uSleep(500); // To buffer some imus before sending images return true; diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index d090eefb..6edc47e2 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -733,6 +733,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->checkbox_stereoMyntEye_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_stereoMyntEye_depth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->checkbox_stereoMyntEye_autoExposure, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_stereoMyntEye_gain, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_stereoMyntEye_brightness, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); + connect(_ui->spinBox_stereoMyntEye_contrast, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel())); connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel())); @@ -1910,6 +1914,10 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->checkbox_stereoRealSense2_odom->setChecked(false); _ui->checkbox_stereoMyntEye_rectify->setChecked(false); _ui->checkbox_stereoMyntEye_depth->setChecked(false); + _ui->checkbox_stereoMyntEye_autoExposure->setChecked(true); + _ui->spinBox_stereoMyntEye_gain->setValue(24); + _ui->spinBox_stereoMyntEye_brightness->setValue(120); + _ui->spinBox_stereoMyntEye_contrast->setValue(116); _ui->checkBox_cameraImages_timestamps->setChecked(false); _ui->checkBox_cameraImages_syncTimeStamps->setChecked(true); @@ -2358,7 +2366,11 @@ void PreferencesDialog::readCameraSettings(const QString & filePath) settings.beginGroup("MyntEye"); _ui->checkbox_stereoMyntEye_rectify->setChecked(settings.value("rectify", _ui->checkbox_stereoMyntEye_rectify->isChecked()).toBool()); _ui->checkbox_stereoMyntEye_depth->setChecked(settings.value("depth", _ui->checkbox_stereoMyntEye_depth->isChecked()).toBool()); - settings.endGroup(); // StereoRealSense2 + _ui->checkbox_stereoMyntEye_autoExposure->setChecked(settings.value("auto_exposure", _ui->checkbox_stereoMyntEye_autoExposure->isChecked()).toBool()); + _ui->spinBox_stereoMyntEye_gain->setValue(settings.value("gain", _ui->spinBox_stereoMyntEye_gain->value()).toInt()); + _ui->spinBox_stereoMyntEye_brightness->setValue(settings.value("brightness", _ui->spinBox_stereoMyntEye_brightness->value()).toInt()); + _ui->spinBox_stereoMyntEye_contrast->setValue(settings.value("contrast", _ui->spinBox_stereoMyntEye_contrast->value()).toInt()); + settings.endGroup(); // MyntEye settings.beginGroup("Images"); _ui->source_images_lineEdit_path->setText(settings.value("path", _ui->source_images_lineEdit_path->text()).toString()); @@ -2828,8 +2840,12 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const settings.endGroup(); // StereoRealSense2 settings.beginGroup("MyntEye"); - settings.setValue("rectify", _ui->checkbox_stereoMyntEye_rectify->isChecked()); - settings.setValue("depth", _ui->checkbox_stereoMyntEye_depth->isChecked()); + settings.setValue("rectify", _ui->checkbox_stereoMyntEye_rectify->isChecked()); + settings.setValue("depth", _ui->checkbox_stereoMyntEye_depth->isChecked()); + settings.setValue("auto_exposure", _ui->checkbox_stereoMyntEye_autoExposure->isChecked()); + settings.setValue("gain", _ui->spinBox_stereoMyntEye_gain->value()); + settings.setValue("brightness", _ui->spinBox_stereoMyntEye_brightness->value()); + settings.setValue("contrast", _ui->spinBox_stereoMyntEye_contrast->value()); settings.endGroup(); // MyntEye settings.beginGroup("Images"); @@ -5678,6 +5694,11 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor) this->getGeneralInputRate(), this->getSourceLocalTransform()); ((CameraMyntEye*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked()); + ((CameraMyntEye*)camera)->setAutoExposure( + _ui->checkbox_stereoMyntEye_autoExposure->isChecked(), + _ui->spinBox_stereoMyntEye_gain->value(), + _ui->spinBox_stereoMyntEye_brightness->value(), + _ui->spinBox_stereoMyntEye_contrast->value()); } } else if(driver == kSrcRGBDImages) diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 250ad0c9..a8e501d9 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - 0 + -687 680 - 2107 + 3270 @@ -95,7 +95,7 @@ QFrame::Raised - 28 + 5 @@ -5339,6 +5339,98 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + + + 240 + + + 120 + + + + + + + 48 + + + 24 + + + + + + + + + + true + + + + + + + Image gain, valid if manual-exposure. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Image brightness, valid if manual-exposure. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Auto-exposure. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + Image contrast, valid if manual-exposure. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 254 + + + 116 + + +