i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-09 04:50:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added Features2d::limitKeypoints() with grid options. Odometry: if imu is provided and no guess is provided, the change of orientation of imu is used for rotation guess (overwrite rotation from Odom/GuessFromMotion). OdometryInfo: added gravity errors when imu is used. Preferences: added a second GravitySigma parameter (overwritting Optimizer/GravitySigma for odometry is not negative) for F2M odometry panel.

Browse Source
This commit is contained in:
matlabbe
2020-05-31 11:22:12 -04:00
parent 6e55525a7b
commit 415a2778f1
15 changed files with 186 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Features2d.h
+3
View File
@@ -183,6 +183,7 @@ public:
static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints);
static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints);
static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols);
static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
@@ -190,6 +191,8 @@ public:
int getMaxFeatures() const {return maxFeatures_;}
float getMinDepth() const {return _minDepth;}
float getMaxDepth() const {return _maxDepth;}
int getGridRows() const {return gridRows_;}
int getGridCols() const {return gridCols_;}
public:
virtual ~Feature2D();
corelib/include/rtabmap/core/Odometry.h
+1
View File
@@ -109,6 +109,7 @@ private:
double previousStamp_;
std::list<std::pair<std::vector<float>, double> > previousVelocities_;
Transform velocityGuess_;
Transform imuLastTransform_;
Transform previousGroundTruthPose_;
float distanceTravelled_;
unsigned int framesProcessed_;
corelib/include/rtabmap/core/OdometryInfo.h
+6
View File
@@ -56,6 +56,8 @@ public:
interval(0),
distanceTravelled(0.0f),
memoryUsage(0),
gravityRollError(0.0),
gravityPitchError(0.0),
type(0)
{}
@@ -84,6 +86,8 @@ public:
output.guessVelocity = guessVelocity;
output.distanceTravelled = distanceTravelled;
output.memoryUsage = memoryUsage;
output.gravityRollError = gravityRollError;
output.gravityPitchError = gravityPitchError;
output.type = type;
return output;
}
@@ -110,6 +114,8 @@ public:
Transform guessVelocity;
float distanceTravelled;
int memoryUsage; //MB
double gravityRollError;
double gravityPitchError;
int type;
corelib/include/rtabmap/core/Statistics.h
+1
View File
@@ -61,6 +61,7 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Reactivate_id,);
RTABMAP_STATS(Loop, Hypothesis_ratio,);
RTABMAP_STATS(Loop, Hypothesis_reactivated,);
RTABMAP_STATS(Loop, Visual_words,);
RTABMAP_STATS(Loop, Visual_inliers,);
RTABMAP_STATS(Loop, Visual_matches,);
RTABMAP_STATS(Loop, Last_id,);
corelib/src/Features2d.cpp
+45 -7
View File
@@ -339,7 +339,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
{
UTimer timer;
ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", (int)keypoints.size());
// Remove words under the new hessian threshold
// Sort words by hessian
@@ -365,10 +365,50 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
}
else
{
ULOGGER_DEBUG("keeping all %d keypoints", (int)keypoints.size());
inliers.resize(keypoints.size(), true);
}
}
void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols)
{
if(maxKeypoints <= 0 || (int)keypoints.size() <= maxKeypoints)
{
inliers.resize(keypoints.size(), true);
return;
}
UASSERT(gridCols>=1 && gridRows >=1);
UASSERT(imageSize.height>gridRows && imageSize.width>gridCols);
int rowSize = imageSize.height / gridRows;
int colSize = imageSize.width / gridCols;
int maxKeypointsPerCell = maxKeypoints / (gridRows * gridCols);
std::vector<std::vector<cv::KeyPoint> > keypointsPerCell(gridRows * gridCols);
std::vector<std::vector<int> > indexesPerCell(gridRows * gridCols);
for(size_t i=0; i<keypoints.size(); ++i)
{
int cellRow = int(keypoints[i].pt.y)/rowSize;
int cellCol = int(keypoints[i].pt.x)/colSize;
UASSERT(cellRow >=0 && cellRow < gridRows);
UASSERT(cellCol >=0 && cellCol < gridCols);
keypointsPerCell[cellRow*gridCols + cellCol].push_back(keypoints[i]);
indexesPerCell[cellRow*gridCols + cellCol].push_back(i);
}
inliers.resize(keypoints.size(), false);
for(size_t i=0; i<keypointsPerCell.size(); ++i)
{
std::vector<bool> inliersCell;
limitKeypoints(keypointsPerCell[i], inliersCell, maxKeypointsPerCell);
for(size_t j=0; j<inliersCell.size(); ++j)
{
if(inliersCell[j])
{
inliers.at(indexesPerCell[i][j]) = true;
}
}
}
}
cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRatios)
{
return util2d::computeRoi(image, roiRatios);
@@ -414,10 +454,6 @@ void Feature2D::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kKpGridCols(), gridCols_);
UASSERT(gridRows_ >= 1 && gridCols_>=1);
if(maxFeatures_ > 0)
{
maxFeatures_ = maxFeatures_ / (gridRows_ * gridCols_);
}
// convert ROI from string to vector
ParametersMap::const_iterator iter;
@@ -653,6 +689,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
// Get keypoints
int rowSize = globalRoi.height / gridRows_;
int colSize = globalRoi.width / gridCols_;
int maxFeatures = maxFeatures_ / (gridRows_ * gridCols_);
for (int i = 0; i<gridRows_; ++i)
{
for (int j = 0; j<gridCols_; ++j)
@@ -660,7 +697,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize);
std::vector<cv::KeyPoint> sub_keypoints;
sub_keypoints = this->generateKeypointsImpl(image, roi, mask);
limitKeypoints(sub_keypoints, maxFeatures_);
limitKeypoints(sub_keypoints, maxFeatures);
if(roi.x || roi.y)
{
// Adjust keypoint position to raw image
@@ -673,7 +710,8 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
keypoints.insert( keypoints.end(), sub_keypoints.begin(), sub_keypoints.end() );
}
}
UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (mask empty=%d)", timer.ticks(), keypoints.size(), mask.empty()?1:0);
UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (grid=%dx%d, mask empty=%d)",
timer.ticks(), keypoints.size(), gridCols_, gridRows_, mask.empty()?1:0);
if(keypoints.size() && _subPixWinSize > 0 && _subPixIterations > 0)
{
corelib/src/Memory.cpp
+22 -4
View File
@@ -4514,10 +4514,26 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
if(_feature2D->getMaxFeatures()>0 && descriptors.rows > _feature2D->getMaxFeatures())
{
UASSERT((int)keypoints.size() == descriptors.rows);
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
int inliersCount = 0;
if(_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1)
{
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(), decimatedData.imageRaw().size(), _feature2D->getGridRows(), _feature2D->getGridCols());
for(size_t i=0; i<inliers.size(); ++i)
{
if(inliers[i])
{
++inliersCount;
}
}
}
else
{
Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
inliersCount = _feature2D->getMaxFeatures();
}
descriptorsForQuantization = cv::Mat(_feature2D->getMaxFeatures(), descriptors.cols, descriptors.type());
quantizedToRawIndices.resize(_feature2D->getMaxFeatures());
descriptorsForQuantization = cv::Mat(inliersCount, descriptors.cols, descriptors.type());
quantizedToRawIndices.resize(inliersCount);
unsigned int oi=0;
UASSERT((int)inliers.size() == descriptors.rows);
for(int k=0; k < descriptors.rows; ++k)
@@ -4537,7 +4553,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
++oi;
}
}
UASSERT((int)oi == _feature2D->getMaxFeatures());
UASSERT_MSG((int)oi == inliersCount,
uFormat("oi=%d inliersCount=%d (maxFeatures=%d, grid=%dx%d)",
oi, inliersCount, _feature2D->getMaxFeatures(), _feature2D->getGridCols(), _feature2D->getGridRows()).c_str());
}
// Quantization to vocabulary
corelib/src/Odometry.cpp
+22
View File
@@ -199,6 +199,7 @@ void Odometry::reset(const Transform & initialPose)
previousStamp_ = 0;
distanceTravelled_ = 0;
framesProcessed_ = 0;
imuLastTransform_.setNull();
if(_force3DoF || particleFilters_.size())
{
float x,y,z, roll,pitch,yaw;
@@ -423,10 +424,29 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
}
Transform imuCurrentTransform;
if(!guessIn.isNull())
{
guess = guessIn;
}
else if(!data.imu().empty())
{
// replace orientation guess with IMU (if available)
if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0))
{
Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
// orientation includes roll and pitch but not yaw in local transform
imuCurrentTransform = Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse();
if(!imuLastTransform_.isNull())
{
orientation = imuLastTransform_.inverse() * imuCurrentTransform;
guess = Transform(
orientation.r11(), orientation.r12(), orientation.r13(), guess.x(),
orientation.r21(), orientation.r22(), orientation.r23(), guess.y(),
orientation.r31(), orientation.r32(), orientation.r33(), guess.z());
}
}
}
UTimer time;
Transform t;
@@ -688,6 +708,8 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
++framesProcessed_;
imuLastTransform_ = imuCurrentTransform;
return _pose *= t; // update
}
else if(_resetCurrentCount > 0)
corelib/src/Rtabmap.cpp
+2
View File
@@ -3132,6 +3132,8 @@ bool Rtabmap::process(
UINFO("Set loop closure transform = %s", loopIter->second.transform().prettyPrint().c_str());
statistics_.setLoopClosureTransform(loopIter->second.transform());
statistics_.addStatistic(Statistics::kLoopVisual_words(), sLoop->getWords().size());
// if ground truth exists, compute localization error
if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull())
{
corelib/src/odometry/OdometryF2M.cpp
+11
View File
@@ -507,6 +507,17 @@ Transform OdometryF2M::computeTransform(
std::multimap<int, Link>::iterator iter = graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false);
UASSERT(iter != bundleLinks.end());
iter->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
iter = graph::findLink(bundleLinks, lastFrame_->id(), lastFrame_->id(), false);
if(info && iter!=bundleLinks.end() && iter->second.type() == Link::kGravity)
{
float rollImu,pitchImu,yaw;
iter->second.transform().getEulerAngles(rollImu, pitchImu, yaw);
float roll,pitch;
transform.getEulerAngles(roll, pitch, yaw);
info->gravityRollError = fabs(rollImu - roll);
info->gravityPitchError = fabs(pitchImu - pitch);
}
}
}
UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
corelib/src/optimizer/OptimizerG2O.cpp
+2 -2
View File
@@ -197,7 +197,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
// Apply g2o optimization
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
//optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
if (isSlam2d())
{
g2o::ParameterSE2Offset* odomOffset = new g2o::ParameterSE2Offset();
@@ -1260,7 +1260,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
if(poses.size()>=2 && iterations() > 0 && (models.size() == poses.size() || poses.begin()->first < 0))
{
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
//optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
#if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM2)
std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
#else
guilib/include/rtabmap/gui/PreferencesDialog.h
+1
View File
@@ -161,6 +161,7 @@ public:
bool isWordsCountGraphView() const;
bool isLocalizationsCountGraphView() const;
int getOdomRegistrationApproach() const;
double getOdomF2MGravitySigma() const;
bool isOdomDisabled() const;
bool isGroundTruthAligned() const;
guilib/src/CloudViewer.cpp
+1 -1
View File
@@ -3345,7 +3345,7 @@ void CloudViewer::handleAction(QAction * a)
else if(a == _aSetGridCellSize)
{
bool ok;
double value = QInputDialog::getDouble(this, tr("Set grid cell size"), tr("Size (m)"), _gridCellSize, 0.01, 10, 2, &ok);
double value = QInputDialog::getDouble(this, tr("Set grid cell size"), tr("Size (m)"), _gridCellSize, 0.01, 1000, 2, &ok);
if(ok)
{
this->setGridCellSize(value);
guilib/src/MainWindow.cpp
+22 -7
View File
@@ -605,6 +605,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", false);
_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", false);
_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", false);
_ui->statsToolBox->updateStat("Odometry/GravityRollError/deg", false);
_ui->statsToolBox->updateStat("Odometry/GravityPitchError/deg", false);
_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", false);
_ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", false);
_ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", false);
@@ -1524,14 +1526,25 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().reg.covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().reg.covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
if(odom.info().timeParticleFiltering>0.0f)
{
_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f, _preferencesDialog->isCacheSavedInFigures());
}
if(odom.info().gravityRollError>0.0f || odom.info().gravityPitchError > 0.0f)
{
_ui->statsToolBox->updateStat("Odometry/GravityRollError/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().gravityRollError*180/M_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/GravityPitchError/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().gravityPitchError*180/M_PI, _preferencesDialog->isCacheSavedInFigures());
}
_ui->statsToolBox->updateStat("Odometry/Features/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().features, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localMapSize, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localScanMapSize, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localKeyFrames, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
if(odom.info().localBundleTime > 0.0f)
{
_ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleOutliers, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleConstraints, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
}
_ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.data().id(), _preferencesDialog->isCacheSavedInFigures());
@@ -1943,9 +1956,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
UDEBUG("time= %d ms", time.restart());
_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the last signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), signature.getWords().size(), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Keypoint/Keypoints count in the loop signature/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), loopSignature.getWords().size(), _preferencesDialog->isCacheSavedInFigures());
// loop closure view
if((stat.loopClosureId() > 0 || stat.proximityDetectionId() > 0) &&
!stat.loopClosureTransform().isNull() &&
@@ -5207,6 +5217,11 @@ void MainWindow::startDetection()
odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry.
int odomStrategy = Parameters::defaultOdomStrategy();
Parameters::parse(odomParameters, Parameters::kOdomStrategy(), odomStrategy);
double gravitySigma = _preferencesDialog->getOdomF2MGravitySigma();
if(gravitySigma >= 0.0)
{
uInsert(odomParameters, ParametersPair(Parameters::kOptimizerGravitySigma(), uNumber2Str(gravitySigma)));
}
if(odomStrategy != 1)
{
// Only Frame To Frame supports all VisCorType
guilib/src/PreferencesDialog.cpp
+8
View File
@@ -421,6 +421,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->radioButton_nochangeGraphView, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteGeneralPanel()));
connect(_ui->checkbox_odomDisabled, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
connect(_ui->odom_registration, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
connect(_ui->odom_f2m_gravitySigma, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteGeneralPanel()));
connect(_ui->checkbox_groundTruthAlign, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
// Cloud rendering panel
@@ -2029,6 +2030,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
if(groupBox->objectName() == _ui->groupBox_odometry1->objectName())
{
_ui->odom_registration->setCurrentIndex(3);
_ui->odom_f2m_gravitySigma->setValue(-1);
}
}
}
@@ -2154,6 +2156,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
_ui->radioButton_nochangeGraphView->setChecked(settings.value("nochangeGraphView", _ui->radioButton_nochangeGraphView->isChecked()).toBool());
_ui->checkbox_odomDisabled->setChecked(settings.value("odomDisabled", _ui->checkbox_odomDisabled->isChecked()).toBool());
_ui->odom_registration->setCurrentIndex(settings.value("odomRegistration", _ui->odom_registration->currentIndex()).toInt());
_ui->odom_f2m_gravitySigma->setValue(settings.value("odomF2MGravitySigma", _ui->odom_f2m_gravitySigma->value()).toDouble());
_ui->checkbox_groundTruthAlign->setChecked(settings.value("gtAlign", _ui->checkbox_groundTruthAlign->isChecked()).toBool());
for(int i=0; i<2; ++i)
@@ -2628,6 +2631,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
settings.setValue("nochangeGraphView", _ui->radioButton_nochangeGraphView->isChecked());
settings.setValue("odomDisabled", _ui->checkbox_odomDisabled->isChecked());
settings.setValue("odomRegistration", _ui->odom_registration->currentIndex());
settings.setValue("odomF2MGravitySigma", _ui->odom_f2m_gravitySigma->value());
settings.setValue("gtAlign", _ui->checkbox_groundTruthAlign->isChecked());
for(int i=0; i<2; ++i)
@@ -5009,6 +5013,10 @@ int PreferencesDialog::getOdomRegistrationApproach() const
{
return _ui->odom_registration->currentIndex();
}
double PreferencesDialog::getOdomF2MGravitySigma() const
{
return _ui->odom_f2m_gravitySigma->value();
}
bool PreferencesDialog::isGroundTruthAligned() const
{
return _ui->checkbox_groundTruthAlign->isChecked();
guilib/src/ui/preferencesDialog.ui
+39 -4
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-687</y>
<y>0</y>
<width>680</width>
<height>3270</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>5</number>
<number>14</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -11004,7 +11004,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="3" column="1">
<widget class="QLabel" name="label_527">
<property name="text">
<string>Gravity sigma value (&gt;=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with GTSAM optimization strategy.</string>
<string>Gravity sigma value (&gt;=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with g2o and GTSAM optimization strategies.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -11020,7 +11020,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<string/>
</property>
<property name="decimals">
<number>2</number>
<number>4</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
@@ -13469,6 +13469,41 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_598">
<property name="text">
<string>[Visual] Gravity sigma used for bundle adjustment (&lt;0, use same value than Optimizer/GravitySigma parameter)</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QDoubleSpinBox" name="odom_f2m_gravitySigma">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>4</number>
</property>
<property name="minimum">
<double>-1.000000000000000</double>
</property>
<property name="maximum">
<double>10.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
<item>