From c7e46c143163c55c6432a1265b9914e6c14d2630 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 25 Jun 2023 17:42:54 -0700 Subject: [PATCH] rgbddatset tool: removed local covariance logic. DBViewer: fixed poses not aligned to groundtruth on start. --- guilib/src/DatabaseViewer.cpp | 46 ++++++++++++++++++++++++----------- tools/RgbdDataset/main.cpp | 18 ++++++-------- 2 files changed, 40 insertions(+), 24 deletions(-) diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index eb1be46e..b77cd2d8 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -1073,8 +1073,11 @@ bool DatabaseViewer::closeDatabase() ui_->toolBox_statistics->clear(); databaseFileName_.clear(); ui_->checkBox_alignPosesWithGPS->setVisible(false); + ui_->checkBox_alignPosesWithGPS->setEnabled(false); ui_->checkBox_alignPosesWithGroundTruth->setVisible(false); + ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false); ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false); + ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false); ui_->doubleSpinBox_optimizationScale->setVisible(false); ui_->label_scale_title->setVisible(false); ui_->label_rmse->setVisible(false); @@ -1706,8 +1709,11 @@ void DatabaseViewer::updateIds() lastSliderIndexBrowsed_ = 0; ui_->checkBox_wmState->setVisible(false); ui_->checkBox_alignPosesWithGPS->setVisible(false); + ui_->checkBox_alignPosesWithGPS->setEnabled(false); ui_->checkBox_alignPosesWithGroundTruth->setVisible(false); + ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false); ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false); + ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false); ui_->doubleSpinBox_optimizationScale->setVisible(false); ui_->label_scale_title->setVisible(false); ui_->label_rmse->setVisible(false); @@ -1903,10 +1909,13 @@ void DatabaseViewer::updateIds() ui_->label_rmse_title->setVisible(!groundTruthPoses_.empty()); ui_->checkBox_alignPosesWithGPS->setVisible(!gpsPoses_.empty()); + ui_->checkBox_alignPosesWithGPS->setEnabled(!gpsPoses_.empty()); ui_->label_alignPosesWithGPS->setVisible(!gpsPoses_.empty()); ui_->checkBox_alignPosesWithGroundTruth->setVisible(!groundTruthPoses_.empty()); + ui_->checkBox_alignPosesWithGroundTruth->setEnabled(!groundTruthPoses_.empty()); ui_->label_alignPosesWithGroundTruth->setVisible(!groundTruthPoses_.empty()); ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(!groundTruthPoses_.empty()); + ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(!groundTruthPoses_.empty()); ui_->label_alignScansCloudsWithGroundTruth->setVisible(!groundTruthPoses_.empty()); if(!gpsValues_.empty()) @@ -2551,11 +2560,12 @@ void DatabaseViewer::exportPoses(int format) optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); } - if(ui_->checkBox_alignPosesWithGPS->isChecked() || - ui_->checkBox_alignPosesWithGroundTruth->isChecked()) + if((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) || + (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) { std::map refPoses = groundTruthPoses_; - if(ui_->checkBox_alignPosesWithGPS->isChecked()) + if(ui_->checkBox_alignPosesWithGPS->isEnabled() && + ui_->checkBox_alignPosesWithGPS->isChecked()) { refPoses = gpsPoses_; } @@ -3162,7 +3172,7 @@ void DatabaseViewer::exportSaved2DMap() file << "free_thresh: 0.196" << std::endl; file << std::endl; file.close(); - + QMessageBox::information(this, tr("Export 2D map"), tr("Exported %1 and %2!").arg(path).arg(yaml)); } @@ -3519,7 +3529,9 @@ void DatabaseViewer::updateOptimizedMesh() } std::map optimizedPoses; - if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty()) + if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() && + ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() + && !groundTruthPoses_.empty()) { optimizedPoses = groundTruthPoses_; } @@ -4036,7 +4048,9 @@ void DatabaseViewer::view3DMap() } std::map optimizedPoses; - if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty()) + if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() && + ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && + !groundTruthPoses_.empty()) { optimizedPoses = groundTruthPoses_; } @@ -4087,7 +4101,9 @@ void DatabaseViewer::generate3DMap() } std::map optimizedPoses; - if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty()) + if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() && + ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && + !groundTruthPoses_.empty()) { optimizedPoses = groundTruthPoses_; } @@ -6665,7 +6681,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) std::map graph = uValueAt(graphes_, value); std::map refPoses = groundTruthPoses_; - if(ui_->checkBox_alignPosesWithGPS->isChecked()) + if(ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) { refPoses = gpsPoses_; } @@ -6729,8 +6745,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) UINFO("rotational_min=%f", rotational_min); UINFO("rotational_max=%f", rotational_max); - if((ui_->checkBox_alignPosesWithGPS->isChecked() || - ui_->checkBox_alignPosesWithGroundTruth->isChecked()) && + if(((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) || + (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) && !gtToMap.isIdentity()) { for(std::map::iterator iter=graph.begin(); iter!=graph.end(); ++iter) @@ -6741,7 +6757,9 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) } std::map graphFiltered; - if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty()) + if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() && + ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && + !groundTruthPoses_.empty()) { graphFiltered = groundTruthPoses_; } @@ -7623,9 +7641,9 @@ void DatabaseViewer::updateGraphView() ui_->spinBox_optimizationsFrom->setEnabled(true); ui_->checkBox_spanAllMaps->setEnabled(true); ui_->checkBox_wmState->setEnabled(true); - ui_->checkBox_alignPosesWithGPS->setEnabled(true); - ui_->checkBox_alignPosesWithGroundTruth->setEnabled(true); - ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(true); + ui_->checkBox_alignPosesWithGPS->setEnabled(ui_->checkBox_alignPosesWithGPS->isVisible()); + ui_->checkBox_alignPosesWithGroundTruth->setEnabled(ui_->checkBox_alignPosesWithGroundTruth->isVisible()); + ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(ui_->checkBox_alignScansCloudsWithGroundTruth->isVisible()); ui_->checkBox_ignoreIntermediateNodes->setEnabled(true); } updateGraphRotation(); diff --git a/tools/RgbdDataset/main.cpp b/tools/RgbdDataset/main.cpp index 82409969..b8c0e757 100644 --- a/tools/RgbdDataset/main.cpp +++ b/tools/RgbdDataset/main.cpp @@ -246,7 +246,6 @@ int main(int argc, char * argv[]) ///////////////////////////// // Processing dataset begin ///////////////////////////// - cv::Mat covariance; int odomKeyFrames = 0; double previousStamp = 0.0; int skipCount = 0; @@ -277,6 +276,12 @@ int main(int argc, char * argv[]) odomInfo.reg.covariance = cv::Mat::eye(6,6,CV_64FC1); } } + + if(iteration!=0 && !odomInfo.reg.covariance.empty() && odomInfo.reg.covariance.at(0,0)>=9999) + { + UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomInfo.reg.covariance.at(0,0)); + rtabmap.triggerNewMap(); + } if(odomInfo.keyFrameAdded) { @@ -303,10 +308,6 @@ int main(int argc, char * argv[]) data.setFeatures(std::vector(), std::vector(), cv::Mat());// remove features processData = intermediateNodes; } - if(covariance.empty() || odomInfo.reg.covariance.at(0,0) > covariance.at(0,0)) - { - covariance = odomInfo.reg.covariance; - } timer.restart(); if(processData) @@ -337,8 +338,7 @@ int main(int argc, char * argv[]) externalStats.insert(std::make_pair("Odometry/LocalScanMapSize/", odomInfo.localScanMapSize)); OdometryEvent e(SensorData(), Transform(), odomInfo); - rtabmap.process(data, pose, covariance, e.velocity(), externalStats); - covariance = cv::Mat(); + rtabmap.process(data, pose, odomInfo.reg.covariance, e.velocity(), externalStats); } ++iteration; @@ -354,8 +354,6 @@ int main(int argc, char * argv[]) if(rmse >= 0.0f) { - //printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm, noise stddev=%fm %frad", - // iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse, sqrt(odomInfo.reg.covariance.at(0,0)), sqrt(odomInfo.reg.covariance.at(3,3))); printf("Iteration %d/%d: camera=%dms, odom(quality=%d/%d, kfs=%d)=%dms, slam=%dms, rmse=%fm", iteration, totalImages, int(cameraInfo.timeTotal*1000.0f), odomInfo.reg.inliers, odomInfo.features, odomKeyFrames, int(odomInfo.timeEstimation*1000.0f), int(slamTime*1000.0f), rmse); } @@ -428,7 +426,7 @@ int main(int argc, char * argv[]) } } - + // compute RMSE statistics float translational_rmse = 0.0f; float translational_mean = 0.0f;