From 379e11bd6682242d8ab89a58a558f0b2f13deda3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 21 Sep 2024 18:14:52 -0700 Subject: [PATCH] Supporting GTSAM bearing/range factors for landmarks (#1346) * Supporting GTSAM bearing/range factors for landmarks * refreshing initial covariance after being modified in same sessions --- corelib/src/optimizer/OptimizerGTSAM.cpp | 39 +- .../rtabmap/gui/EditConstraintDialog.h | 8 +- guilib/src/DatabaseViewer.cpp | 87 +-- guilib/src/EditConstraintDialog.cpp | 51 +- guilib/src/ui/editConstraintDialog.ui | 613 +++++++++++------- tools/Report/main.cpp | 54 +- 6 files changed, 528 insertions(+), 324 deletions(-) diff --git a/corelib/src/optimizer/OptimizerGTSAM.cpp b/corelib/src/optimizer/OptimizerGTSAM.cpp index 1c540b8f54..31eafd647a 100644 --- a/corelib/src/optimizer/OptimizerGTSAM.cpp +++ b/corelib/src/optimizer/OptimizerGTSAM.cpp @@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -551,7 +552,7 @@ std::map OptimizerGTSAM::optimize( lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); } } - else + else if(1 / static_cast(iter->second.infMatrix().at(1,1)) < 9999) { Eigen::Matrix information = Eigen::Matrix::Identity(); if(!isCovarianceIgnored()) @@ -566,6 +567,21 @@ std::map OptimizerGTSAM::optimize( graph.add(gtsam::BearingRangeFactor(id1, id2, p.bearing(landmark), p.range(landmark), model)); lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); } + else + { + Eigen::Matrix information = Eigen::Matrix::Identity(); + if(!isCovarianceIgnored()) + { + cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,1), cv::Range(0,1)).clone();; + memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double)); + } + gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information); + + gtsam::Point2 landmark(t.x(), t.y()); + gtsam::Pose2 p; + graph.add(gtsam::BearingFactor(id1, id2, p.bearing(landmark), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); + } } else { @@ -599,14 +615,15 @@ std::map OptimizerGTSAM::optimize( lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); } } - else + else if(1 / static_cast(iter->second.infMatrix().at(2,2)) < 9999) { Eigen::Matrix information = Eigen::Matrix::Identity(); if(!isCovarianceIgnored()) { - cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone();; + cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,3), cv::Range(0,3)).clone(); memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double)); } + gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information); gtsam::Point3 landmark(t.x(), t.y(), t.z()); @@ -614,6 +631,22 @@ std::map OptimizerGTSAM::optimize( graph.add(gtsam::BearingRangeFactor(id1, id2, p.bearing(landmark), p.range(landmark), model)); lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); } + else + { + Eigen::Matrix information = Eigen::Matrix::Identity(); + if(!isCovarianceIgnored()) + { + cv::Mat linearCov = cv::Mat(iter->second.infMatrix(), cv::Range(0,2), cv::Range(0,2)).clone(); + memcpy(information.data(), linearCov.data, linearCov.total()*sizeof(double)); + } + + gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(information); + + gtsam::Point3 landmark(t.x(), t.y(), t.z()); + gtsam::Pose3 p; + graph.add(gtsam::BearingFactor(id1, id2, p.bearing(landmark), model)); + lastAddedConstraints_.push_back(ConstraintToFactor(id1, id2, -1)); + } } } } diff --git a/guilib/include/rtabmap/gui/EditConstraintDialog.h b/guilib/include/rtabmap/gui/EditConstraintDialog.h index 8a37be8e76..fd9a5e774b 100644 --- a/guilib/include/rtabmap/gui/EditConstraintDialog.h +++ b/guilib/include/rtabmap/gui/EditConstraintDialog.h @@ -42,12 +42,14 @@ class RTABMAP_GUI_EXPORT EditConstraintDialog : public QDialog Q_OBJECT public: - EditConstraintDialog(const Transform & constraint, double linearSigma = 1, double angularSigma = 1, QWidget * parent = 0); + EditConstraintDialog(const Transform & constraint, const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1), QWidget * parent = 0); + + void setPoseGroupVisible(bool visible); + void setCovarianceGroupVisible(bool visible); virtual ~EditConstraintDialog(); Transform getTransform() const; - double getLinearVariance() const; - double getAngularVariance() const; + cv::Mat getCovariance() const; private Q_SLOTS: void switchUnits(); diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 5b79e92cb3..9d6dbb96a8 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -4357,13 +4357,19 @@ void DatabaseViewer::updateCovariances(const QList & links) { if(links.size()) { - bool ok = false; - double stddev = QInputDialog::getDouble(this, tr("Linear error"), tr("Std deviation (m) 0=inf"), 0.01, 0.0, 9, 4, &ok); - if(!ok) return; - double linearVar = stddev*stddev; - stddev = QInputDialog::getDouble(this, tr("Angular error"), tr("Std deviation (deg) 0=inf"), 1, 0.0, 90, 2, &ok)*M_PI/180.0; - if(!ok) return; - double angularVar = stddev*stddev; + cv::Mat infMatrix = links.first().infMatrix(); + std::multimap::iterator findIter = rtabmap::graph::findLink(linksRefined_, links.first().from() ,links.first().to(), false, links.first().type()); + if(findIter != linksRefined_.end()) + { + infMatrix = findIter->second.infMatrix(); + } + + EditConstraintDialog dialog(Transform::getIdentity(), infMatrix.inv()); + dialog.setPoseGroupVisible(false); + if(dialog.exec() != QDialog::Accepted) + { + return; + } rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this); progressDialog->setAttribute(Qt::WA_DeleteOnClose); @@ -4372,23 +4378,7 @@ void DatabaseViewer::updateCovariances(const QList & links) progressDialog->setMinimumWidth(800); progressDialog->show(); - cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1); - if(linearVar == 0.0) - { - infMatrix(cv::Range(0,3), cv::Range(0,3)) /= 9999.9; - } - else - { - infMatrix(cv::Range(0,3), cv::Range(0,3)) /= linearVar; - } - if(angularVar == 0.0) - { - infMatrix(cv::Range(3,6), cv::Range(3,6)) /= 9999.9; - } - else - { - infMatrix(cv::Range(3,6), cv::Range(3,6)) /= angularVar; - } + infMatrix = dialog.getCovariance().inv(); for(int i=0; ilabel_type->text().toInt() == Link::kLandmark) { link = loopLinks_.at(ui_->horizontalSlider_loops->value()); + std::multimap::iterator findIter = rtabmap::graph::findLink(linksRefined_, link.from() ,link.to(), false, link.type()); + if(findIter != linksRefined_.end()) + { + link = findIter->second; + } } else { @@ -6083,28 +6078,10 @@ void DatabaseViewer::editConstraint() if(link.isValid()) { cv::Mat covBefore = link.infMatrix().inv(); - EditConstraintDialog dialog(link.transform(), - covBefore.at(0,0)<9999.0?std::sqrt(covBefore.at(0,0)):0.0, - covBefore.at(5,5)<9999.0?std::sqrt(covBefore.at(5,5)):0.0); + EditConstraintDialog dialog(link.transform(), covBefore); if(dialog.exec() == QDialog::Accepted) { - cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1); - if(dialog.getLinearVariance()>0) - { - covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance(); - } - else - { - covariance(cv::Range(0,3), cv::Range(0,3)) *= 9999.9; - } - if(dialog.getAngularVariance()>0) - { - covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance(); - } - else - { - covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9; - } + cv::Mat covariance = dialog.getCovariance(); Link newLink(link.from(), link.to(), link.type(), dialog.getTransform(), covariance.inv()); std::multimap::iterator iter = linksRefined_.find(link.from()); while(iter != linksRefined_.end() && iter->first == link.from()) @@ -6133,28 +6110,10 @@ void DatabaseViewer::editConstraint() else { EditConstraintDialog dialog( - link.transform(), - priorId>0?0.001:1, - priorId>0?0.001:1); + link.transform(), cv::Mat::eye(6,6,CV_64FC1) * (priorId>0?0.00001:1)); if(dialog.exec() == QDialog::Accepted) { - cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1); - if(dialog.getLinearVariance()>0) - { - covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance(); - } - else - { - covariance(cv::Range(0,3), cv::Range(0,3)) *= 9999.9; - } - if(dialog.getAngularVariance()>0) - { - covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance(); - } - else - { - covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9; - } + cv::Mat covariance = dialog.getCovariance(); int from = priorId>0?priorId:ids_.at(ui_->horizontalSlider_A->value()); int to = priorId>0?priorId:ids_.at(ui_->horizontalSlider_B->value()); Link newLink( diff --git a/guilib/src/EditConstraintDialog.cpp b/guilib/src/EditConstraintDialog.cpp index 59229a46f6..88e0bed909 100644 --- a/guilib/src/EditConstraintDialog.cpp +++ b/guilib/src/EditConstraintDialog.cpp @@ -28,13 +28,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/gui/EditConstraintDialog.h" #include "ui_editConstraintDialog.h" +#include +#include + #ifndef M_PI #define M_PI 3.14159265358979323846 #endif namespace rtabmap { -EditConstraintDialog::EditConstraintDialog(const Transform & constraint, double linearSigma, double angularSigma, QWidget * parent) : +EditConstraintDialog::EditConstraintDialog(const Transform & constraint, const cv::Mat & covariance, QWidget * parent) : QDialog(parent) { _ui = new Ui_EditConstraintDialog(); @@ -49,9 +52,15 @@ EditConstraintDialog::EditConstraintDialog(const Transform & constraint, double _ui->pitch->setValue(pitch); _ui->yaw->setValue(yaw); + UASSERT(covariance.empty() || (covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)); + _ui->checkBox_radians->setChecked(true); - _ui->linear_sigma->setValue(linearSigma); - _ui->angular_sigma->setValue(angularSigma); + _ui->linear_sigma_x->setValue(covariance.empty() || covariance.at(0,0)>=9999 || covariance.at(0,0)<=0?0:sqrt(covariance.at(0,0))); + _ui->linear_sigma_y->setValue(covariance.empty() || covariance.at(1,1)>=9999 || covariance.at(1,1)<=0?0:sqrt(covariance.at(1,1))); + _ui->linear_sigma_z->setValue(covariance.empty() || covariance.at(2,2)>=9999 || covariance.at(2,2)<=0?0:sqrt(covariance.at(2,2))); + _ui->angular_sigma_roll->setValue(covariance.empty() || covariance.at(3,3)>=9999 || covariance.at(3,3)<=0?0:sqrt(covariance.at(3,3))); + _ui->angular_sigma_pitch->setValue(covariance.empty() || covariance.at(4,4)>=9999 || covariance.at(4,4)<=0?0:sqrt(covariance.at(4,4))); + _ui->angular_sigma_yaw->setValue(covariance.empty() || covariance.at(5,5)>=9999 || covariance.at(5,5)<=0?0:sqrt(covariance.at(5,5))); connect(_ui->checkBox_radians, SIGNAL(stateChanged(int)), this, SLOT(switchUnits())); } @@ -61,6 +70,15 @@ EditConstraintDialog::~EditConstraintDialog() delete _ui; } +void EditConstraintDialog::setPoseGroupVisible(bool visible) +{ + _ui->groupBox_pose->setVisible(visible); +} +void EditConstraintDialog::setCovarianceGroupVisible(bool visible) +{ + _ui->groupBox_covariance->setVisible(visible); +} + void EditConstraintDialog::switchUnits() { double conversion = 180.0/M_PI; @@ -72,13 +90,15 @@ void EditConstraintDialog::switchUnits() boxes.push_back(_ui->roll); boxes.push_back(_ui->pitch); boxes.push_back(_ui->yaw); - boxes.push_back(_ui->angular_sigma); + boxes.push_back(_ui->angular_sigma_roll); + boxes.push_back(_ui->angular_sigma_pitch); + boxes.push_back(_ui->angular_sigma_yaw); for(int i=0; ivalue()*conversion; if(_ui->checkBox_radians->isChecked()) { - if(boxes[i]!=_ui->angular_sigma) + if(boxes[i]!=_ui->angular_sigma_roll && boxes[i]!=_ui->angular_sigma_pitch && boxes[i]!=_ui->angular_sigma_yaw) { boxes[i]->setMinimum(-M_PI); } @@ -88,7 +108,7 @@ void EditConstraintDialog::switchUnits() } else { - if(boxes[i]!=_ui->angular_sigma) + if(boxes[i]!=_ui->angular_sigma_roll && boxes[i]!=_ui->angular_sigma_pitch && boxes[i]!=_ui->angular_sigma_yaw) { boxes[i]->setMinimum(-180); } @@ -110,19 +130,24 @@ Transform EditConstraintDialog::getTransform() const return Transform(_ui->x->value(), _ui->y->value(), _ui->z->value(), _ui->roll->value()*conversion, _ui->pitch->value()*conversion, _ui->yaw->value()*conversion); } -double EditConstraintDialog::getLinearVariance() const -{ - return _ui->linear_sigma->value()*_ui->linear_sigma->value(); -} -double EditConstraintDialog::getAngularVariance() const +cv::Mat EditConstraintDialog::getCovariance() const { + cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); + covariance.at(0,0) = _ui->linear_sigma_x->value()==0?9999:_ui->linear_sigma_x->value()*_ui->linear_sigma_x->value(); + covariance.at(1,1) = _ui->linear_sigma_y->value()==0?9999:_ui->linear_sigma_y->value()*_ui->linear_sigma_y->value(); + covariance.at(2,2) = _ui->linear_sigma_z->value()==0?9999:_ui->linear_sigma_z->value()*_ui->linear_sigma_z->value(); double conversion = 1.0f; if(!_ui->checkBox_radians->isChecked()) { conversion = M_PI/180.0; } - double value = _ui->angular_sigma->value()*conversion; - return value*value; + double sigma = _ui->angular_sigma_roll->value()*conversion; + covariance.at(3,3) = sigma==0?9999:sigma*sigma; + sigma = _ui->angular_sigma_pitch->value()*conversion; + covariance.at(4,4) = sigma==0?9999:sigma*sigma; + sigma = _ui->angular_sigma_yaw->value()*conversion; + covariance.at(5,5) = sigma==0?9999:sigma*sigma; + return covariance; } } diff --git a/guilib/src/ui/editConstraintDialog.ui b/guilib/src/ui/editConstraintDialog.ui index afa258561e..9c13bcb2e9 100644 --- a/guilib/src/ui/editConstraintDialog.ui +++ b/guilib/src/ui/editConstraintDialog.ui @@ -6,8 +6,8 @@ 0 0 - 430 - 231 + 393 + 360 @@ -15,232 +15,352 @@ - - - - - x - - - - - - - m - - - 6 - - - -9999.000000000000000 - - - 9999.000000000000000 - - - 0.001000000000000 - - - - - - - roll - - - - - - - rad - - - 6 - - - -3.150000000000000 - - - 3.150000000000000 - - - 0.001000000000000 - - - - - - - y - - - - - - - m - - - 6 - - - -9999.000000000000000 - - - 9999.000000000000000 - - - 0.001000000000000 - - - - - - - pitch - - - - - - - rad - - - 6 - - - -3.150000000000000 - - - 3.150000000000000 - - - 0.001000000000000 - - - - - - - z - - - - - - - m - - - 6 - - - -9999.000000000000000 - - - 9999.000000000000000 - - - 0.001000000000000 - - - - - - - yaw - - - - - - - rad - - - 6 - - - -3.150000000000000 - - - 3.150000000000000 - - - 0.001000000000000 - - - - - - - m - - - 6 - - - 0.000000000000000 - - - 9999.000000000000000 - - - 0.001000000000000 - - - 0.000000000000000 - - - - - - - rad - - - 6 - - - 0.000000000000000 - - - 3.150000000000000 - - - 0.001000000000000 - - - 0.000000000000000 - - - - - - - <html><head/><body><p>Linear &sigma; </p></body></html> - - - - - - - <html><head/><body><p>Angular σ</p></body></html> - - - - - - - Radians - - - true - - - - + + + Pose + + + + + + x + + + + + + + m + + + 6 + + + -9999.000000000000000 + + + 9999.000000000000000 + + + 0.001000000000000 + + + + + + + roll + + + + + + + rad + + + 6 + + + -3.150000000000000 + + + 3.150000000000000 + + + 0.001000000000000 + + + + + + + y + + + + + + + m + + + 6 + + + -9999.000000000000000 + + + 9999.000000000000000 + + + 0.001000000000000 + + + + + + + pitch + + + + + + + rad + + + 6 + + + -3.150000000000000 + + + 3.150000000000000 + + + 0.001000000000000 + + + + + + + z + + + + + + + m + + + 6 + + + -9999.000000000000000 + + + 9999.000000000000000 + + + 0.001000000000000 + + + + + + + yaw + + + + + + + rad + + + 6 + + + -3.150000000000000 + + + 3.150000000000000 + + + 0.001000000000000 + + + + + + + + + + Covariance + + + + + + rad + + + 6 + + + 0.000000000000000 + + + 3.150000000000000 + + + 0.001000000000000 + + + 0.000000000000000 + + + + + + + <html><head/><body><p>σ pitch</p></body></html> + + + + + + + <html><head/><body><p>σ x</p></body></html> + + + + + + + <html><head/><body><p>σ roll</p></body></html> + + + + + + + rad + + + 6 + + + 0.000000000000000 + + + 3.150000000000000 + + + 0.001000000000000 + + + 0.000000000000000 + + + + + + + m + + + 6 + + + 0.000000000000000 + + + 9999.000000000000000 + + + 0.001000000000000 + + + 0.000000000000000 + + + + + + + rad + + + 6 + + + 0.000000000000000 + + + 3.150000000000000 + + + 0.001000000000000 + + + 0.000000000000000 + + + + + + + m + + + 6 + + + 0.000000000000000 + + + 9999.000000000000000 + + + 0.001000000000000 + + + 0.000000000000000 + + + + + + + m + + + 6 + + + 0.000000000000000 + + + 9999.000000000000000 + + + 0.001000000000000 + + + 0.000000000000000 + + + + + + + <html><head/><body><p>σ yaw</p></body></html> + + + + + + + <html><head/><body><p>σ y</p></body></html> + + + + + + + <html><head/><body><p>σ z</p></body></html> + + + + + @@ -256,11 +376,38 @@ - - - <html><head/><body><p>Setting σ to 0 will set 9999 covariance.</p></body></html> - - + + + + + <html><head/><body><p>Setting σ to 0 will set 9999 covariance.</p></body></html> + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + Radians + + + true + + + + diff --git a/tools/Report/main.cpp b/tools/Report/main.cpp index 791c9e9a0d..2b191a0b1d 100644 --- a/tools/Report/main.cpp +++ b/tools/Report/main.cpp @@ -88,6 +88,7 @@ void showUsage() " --loc_delay # Delay to split sessions for localization statistics (default 60 seconds).\n" " --ignore_inter_nodes Ignore intermediate poses and statistics.\n" " --udebug Show debug log.\n" + " --\"parameter name\" \"value\" Overwrite a specific RTAB-Map's parameter.\n" " --help,-h Show usage\n\n"); exit(1); } @@ -159,6 +160,7 @@ int main(int argc, char * argv[]) #ifdef WITH_QT std::map figures; #endif + ParametersMap overriddenParams = Parameters::parseArguments(argc, argv, true); for(int i=1; ifirst.c_str(), iter->second.c_str()); + } + } + std::string path = argv[argc-1]; path = uReplaceChar(path, '~', UDirectory::homeDir()); @@ -498,6 +513,7 @@ int main(int argc, char * argv[]) ULogger::setLevel(ULogger::kError); // to suppress parameter warnings } params = driver->getLastParameters(); + uInsert(params, overriddenParams); ULogger::setLevel(logLevel); std::set ids; driver->getAllNodeIds(ids, false, false, ignoreInterNodes); @@ -626,6 +642,9 @@ int main(int argc, char * argv[]) std::map > localizationSessionStats; double previousStamp = 0.0; std::map allWeights; + Transform previousPose; + int previousMapId = 0; + float totalOdomDistance = 0.0f; for(std::set::iterator iter=allIds.begin(); iter!=allIds.end(); ++iter) { Transform p, gt; @@ -637,8 +656,18 @@ int main(int argc, char * argv[]) EnvSensors sensors; if(driver->getNodeInfo(*iter, p, m, w, l, s, gt, v, gps, sensors)) { + if(previousMapId == m) + { + if(!p.isNull() && !previousPose.isNull()) + { + totalOdomDistance += p.getDistance(previousPose); + } + } + previousPose = p; + previousMapId = m; + allWeights.insert(std::make_pair(*iter, w)); - if((!ignoreInterNodes || w!=-1)) + if((!ignoreInterNodes || w!=-1) && w!=-9) { odomPoses.insert(std::make_pair(*iter, p)); odomStamps.insert(std::make_pair(*iter, s)); @@ -804,6 +833,7 @@ int main(int argc, char * argv[]) allLinks=allBiLinks; } std::multimap loopClosureLinks; + int landmarks = 0; for(std::multimap::iterator jter=allLinks.begin(); jter!=allLinks.end(); ++jter) { if(jter->second.from() == jter->second.to() || graph::findLink(links, jter->second.from(), jter->second.to(), true) == links.end()) @@ -839,10 +869,16 @@ int main(int argc, char * argv[]) } if( jter->second.type() != Link::kNeighbor && jter->second.type() != Link::kNeighborMerged && + jter->second.type() != Link::kGravity && + jter->second.type() != Link::kLandmark && graph::findLink(loopClosureLinks, jter->second.from(), jter->second.to()) == loopClosureLinks.end()) { loopClosureLinks.insert(*jter); } + else if(jter->second.type() == Link::kLandmark) + { + landmarks++; + } } float bestScale = 1.0f; @@ -1173,10 +1209,11 @@ int main(int argc, char * argv[]) } } } - printf(" %s (%d, s=%.3f):\terror lin=%.3fm (max=%s, odom=%.3fm) ang=%.1fdeg%s%s, %s: avg=%dms (max=%dms) loops=%d%s, odom: avg=%dms (max=%dms), camera: avg=%dms, %smap=%dMB\n", + printf(" %s (%d, %.1f m%s): RMSE= %.3f m (max=%s, odom=%.3f m) ang=%.1f deg%s%s, %s: avg=%d ms (max=%d ms) loops=%d%s%s%s%s%s%s\n", fileName.c_str(), (int)ids.size(), - bestScale, + totalOdomDistance, + bestScale != 1.0f?uFormat(", s=%.3f", bestScale).c_str():"", bestRMSE, maxRMSE!=-1?uFormat("%.3fm", maxRMSE).c_str():"NA", bestVoRMSE, @@ -1186,11 +1223,12 @@ int main(int argc, char * argv[]) !localizationMultiStats.empty()?"loc":"slam", (int)uMean(slamTime), (int)uMax(slamTime), (int)loopClosureLinks.size(), + landmarks==0?"":uFormat(", landmarks = %d", landmarks).c_str(), !outputLoopAccuracy?"":uFormat(" (t_err=%.3fm r_err=%.2f deg)", loop_t_err, loop_r_err).c_str(), - (int)uMean(odomTime), (int)uMax(odomTime), - (int)uMean(cameraTime), - maxOdomRAM!=-1.0f?uFormat("RAM odom=%dMB ", (int)maxOdomRAM).c_str():"", - (int)maxMapRAM); + odomTime.empty()?"":uFormat(", odom: avg=%dms (max=%dms)", (int)uMean(odomTime), (int)uMax(odomTime)).c_str(), + cameraTime.empty()?"":uFormat(", camera: avg=%dms", (int)uMean(cameraTime)).c_str(), + maxOdomRAM!=-1.0f?uFormat(", RAM odom=%dMB ", (int)maxOdomRAM).c_str():"", + maxMapRAM!=-1.0f?uFormat(", map=%dMB",(int)maxMapRAM).c_str():""); if(outputLatex) {