diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index da85efcc0c..5fb698294e 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -104,7 +104,20 @@ bool exportPoses( #endif if(fout) { - for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) + std::list > posesList; + for(std::map::const_iterator iter=poses.lower_bound(0); iter!=poses.end(); ++iter) + { + posesList.push_back(*iter); + } + if(format == 11) + { + // Put landmarks at the end + for(std::map::const_iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0; ++iter) + { + posesList.push_back(*iter); + } + } + for(std::list >::const_iterator iter=posesList.begin(); iter!=posesList.end(); ++iter) { if(format == 1 || format == 10 || format == 11) // rgbd-slam format { @@ -125,7 +138,7 @@ bool exportPoses( // Format: stamp x y z qx qy qz qw Eigen::Quaternionf q = pose.getQuaternionf(); - if(iter == poses.begin()) + if(iter == posesList.begin()) { // header if(format == 11) 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 c72342ebf6..ee330cb7ce 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -4379,13 +4379,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); @@ -4394,23 +4400,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 { @@ -6115,28 +6110,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()) @@ -6165,28 +6142,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..8e7b057d5c 100644 --- a/tools/Report/main.cpp +++ b/tools/Report/main.cpp @@ -60,8 +60,9 @@ void showUsage() " --loop Compute relative motion error of loop closures.\n" " --scale Find the best scale for the map against the ground truth\n" " and compute error based on the scaled path.\n" - " --poses Export poses to [path]_poses.txt, ground truth to [path]_gt.txt\n" - " and valid ground truth indices to [path]_indices.txt \n" + " --poses Export odometry to [path]_odom.txt, optimized graph to [path]_slam.txt \n" + " and ground truth to [path]_gt.txt in TUM RGB-D format.\n" + " --poses_raw Same as --poses, but poses are not aligned to gt." " --gt FILE.txt Use this file as ground truth (TUM RGB-D format). It will\n" " override the ground truth set in database if there is one.\n" " If extension is *.db, the optimized poses of that database will\n" @@ -88,6 +89,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); } @@ -143,6 +145,7 @@ int main(int argc, char * argv[]) bool outputRelativeError = false; bool outputReport = false; bool outputLoopAccuracy = false; + bool outputPosesAlignedToGt = true; bool incrementalOptimization = false; bool showAvailableStats = false; bool invertFigures = false; @@ -159,6 +162,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 +520,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 +649,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 +663,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 +840,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 +876,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; @@ -850,6 +893,7 @@ int main(int argc, char * argv[]) float bestRMSEAng = -1; float bestVoRMSE = -1; Transform bestGtToMap = Transform::getIdentity(); + Transform bestGtToOdom = Transform::getIdentity(); float kitti_t_err = 0.0f; float kitti_r_err = 0.0f; float relative_t_err = 0.0f; @@ -908,10 +952,20 @@ int main(int argc, char * argv[]) { //remove landmarks std::map::iterator iter=poses.begin(); + std::map optimizedLandmarks; while(iter!=poses.end() && iter->first < 0) { + optimizedLandmarks.insert(*iter); poses.erase(iter++); } + if(outputKittiError) { + // remove landmarks + std::map::iterator iter=posesOut.begin(); + while(iter!=posesOut.end() && iter->first < 0) + { + posesOut.erase(iter++); + } + } std::map groundTruth; for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) @@ -957,7 +1011,7 @@ int main(int argc, char * argv[]) float rotational_min = 0.0f; float rotational_max = 0.0f; - graph::calcRMSE( + Transform gtToOdom = graph::calcRMSE( groundTruth, scaledOdomPoses, translational_rmse, @@ -999,6 +1053,7 @@ int main(int argc, char * argv[]) bestRMSEAng = rotational_rmse; bestScale = scale; bestGtToMap = gtToMap; + bestGtToOdom = gtToOdom; if(!outputScaled) { // just did iteration without any scale, then exit @@ -1006,12 +1061,32 @@ int main(int argc, char * argv[]) } } + // Scale/align slam poses for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { iter->second.x()*=bestScale; iter->second.y()*=bestScale; iter->second.z()*=bestScale; - iter->second = bestGtToMap * iter->second; + if(outputPosesAlignedToGt) + iter->second = bestGtToMap * iter->second; + } + for(std::map::iterator iter=optimizedLandmarks.begin(); iter!=optimizedLandmarks.end(); ++iter) + { + iter->second.x()*=bestScale; + iter->second.y()*=bestScale; + iter->second.z()*=bestScale; + if(outputPosesAlignedToGt) + iter->second = bestGtToMap * iter->second; + } + + // Scale/align odom poses + for(std::map::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter) + { + iter->second.x()*=bestScale; + iter->second.y()*=bestScale; + iter->second.z()*=bestScale; + if(outputPosesAlignedToGt) + iter->second = bestGtToOdom * iter->second; } if(outputRelativeError) @@ -1063,14 +1138,26 @@ int main(int argc, char * argv[]) std::multimap dummyLinks; std::map stamps; if(!outputKittiError) + { + // re-add landmarks + uInsert(poses, optimizedLandmarks); + } + if(!outputKittiError) { for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { - UASSERT(odomStamps.find(iter->first) != odomStamps.end()); - stamps.insert(*odomStamps.find(iter->first)); + if(iter->first < 0) + { + stamps.insert(std::make_pair(iter->first, 0)); + } + else + { + UASSERT(odomStamps.find(iter->first) != odomStamps.end()); + stamps.insert(*odomStamps.find(iter->first)); + } } } - if(!graph::exportPoses(path, outputKittiError?2:10, poses, dummyLinks, stamps)) + if(!graph::exportPoses(path, outputKittiError?2:11, poses, dummyLinks, stamps)) { printf("Could not export the poses to \"%s\"!?!\n", path.c_str()); } @@ -1080,13 +1167,20 @@ int main(int argc, char * argv[]) stamps.clear(); if(!outputKittiError) { - for(std::map::iterator iter=odomPoses.begin(); iter!=odomPoses.end(); ++iter) + for(std::map::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter) { - UASSERT(odomStamps.find(iter->first) != odomStamps.end()); - stamps.insert(*odomStamps.find(iter->first)); + if(iter->first < 0) + { + stamps.insert(std::make_pair(iter->first, 0)); + } + else + { + UASSERT(odomStamps.find(iter->first) != odomStamps.end()); + stamps.insert(*odomStamps.find(iter->first)); + } } } - if(!graph::exportPoses(path, outputKittiError?2:10, odomPoses, dummyLinks, stamps)) + if(!graph::exportPoses(path, outputKittiError?2:11, posesOut, dummyLinks, stamps)) { printf("Could not export the odometry to \"%s\"!?!\n", path.c_str()); } @@ -1104,7 +1198,7 @@ int main(int argc, char * argv[]) stamps.insert(*odomStamps.find(iter->first)); } } - if(!graph::exportPoses(path, outputKittiError?2:10, groundTruth, dummyLinks, stamps)) + if(!graph::exportPoses(path, outputKittiError?2:11, groundTruth, dummyLinks, stamps)) { printf("Could not export the ground truth to \"%s\"!?!\n", path.c_str()); } @@ -1173,10 +1267,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 +1281,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) {