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

0.11.13. OdometryF2M: fixed/improved bundle adjustment option. OptimizerG2O: now doing stereo BA instead of mono BA (interface of Optimizer::optimzeBA() has slightly changed to include depth in word references). Registration and Odometry set angular and linear variances separately. RegistrationVis set x100 smaller variance for rotation. Added keypoints3D member to SensorData. Added Transform::getAngle() for convenience. Odometry normalizes variances. RtabmapThread does variance summation when frames are discarded. Updated how variance is computed by util3d::estimateMotion3Dto2D(), ignoring very large variance from the computation (stereo issue with very far matched features). MainWindow: added option to align with ground truth or not (default true as before), added for odometry statistics (including bundle stuff), update ground truth statistics every time a graph is loaded/optimized. CameraRGB can now load odometry files (to fake an input odometry). PreferencesDialog: Added option to Odometry so it can use a different registration approach than the default one (the one used for loop closure). Parameters: added new g2o/Solver option 3 (Eigen), added g2o/RobustKernelDelta, g2o/Baseline and Odom/VisKeyFrameThr. Database: from 0.11.13, ignoring patch version when comparing is database version is newer than app version used.

Browse Source
This commit is contained in:
matlabbe
2017-01-03 21:05:00 -05:00
parent 6550a52cee
commit 6abed48af6
48 changed files with 2202 additions and 1329 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
guilib/src/CloudViewer.cpp
View File

@@ -197,11 +197,16 @@ void CloudViewer::clear()
this->removeAllLines();
this->removeAllFrustums();
this->removeAllTexts();
this->clearTrajectory();
this->removeOccupancyGridMap();
this->removeOctomap();
this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2);
_lastPose.setNull();
if(_aLockCamera->isChecked() || _aFollowCamera->isChecked())
{
resetCamera();
}
this->clearTrajectory();
}
void CloudViewer::createMenu()

13
guilib/src/DatabaseViewer.cpp
View File

@@ -3736,7 +3736,10 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
}
else if(localMaps_.find(ids[i]) != localMaps_.end())
{
localMaps.insert(*localMaps_.find(ids.at(i)));
if(!localMaps_.find(ids[i])->second.first.empty() || !localMaps_.find(ids[i])->second.first.empty())
{
localMaps.insert(*localMaps_.find(ids.at(i)));
}
}
else
{
@@ -3745,6 +3748,10 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
cv::Mat ground, obstacles;
data.uncompressData(0, 0, 0, 0, &ground, &obstacles);
localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
if(!ground.empty() || !obstacles.empty())
{
localMaps.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
}
}
}
//cleanup
@@ -4187,7 +4194,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
if(!transform.isNull())
{
Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.variance, info.variance);
Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.varianceAng, info.varianceLin);
bool updated = false;
std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
@@ -4285,7 +4292,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGra
if(!t.isNull())
{
newLink = Link(from, to, Link::kUserClosure, t, info.variance, info.variance);
newLink = Link(from, to, Link::kUserClosure, t, info.varianceAng, info.varianceLin);
}
else if(!silent)
{

341
guilib/src/MainWindow.cpp
View File

@@ -553,14 +553,23 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->statsToolBox->updateStat("Odometry/ID/", false);
_ui->statsToolBox->updateStat("Odometry/Features/", false);
_ui->statsToolBox->updateStat("Odometry/Matches/", false);
_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", false);
_ui->statsToolBox->updateStat("Odometry/Inliers/", false);
_ui->statsToolBox->updateStat("Odometry/InliersRatio/", false);
_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", false);
_ui->statsToolBox->updateStat("Odometry/StdDev/", false);
_ui->statsToolBox->updateStat("Odometry/Variance/", false);
_ui->statsToolBox->updateStat("Odometry/StdDevLin/", false);
_ui->statsToolBox->updateStat("Odometry/StdDevAng/", false);
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", false);
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", false);
_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", false);
_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", false);
_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", false);
_ui->statsToolBox->updateStat("Odometry/LocalScanMapSize/", false);
_ui->statsToolBox->updateStat("Odometry/LocalKeyFrames/", false);
_ui->statsToolBox->updateStat("Odometry/localBundleOutliers/", false);
_ui->statsToolBox->updateStat("Odometry/localBundleConstraints/", false);
_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", false);
_ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", false);
_ui->statsToolBox->updateStat("Odometry/Interval/ms", false);
_ui->statsToolBox->updateStat("Odometry/Speed/kph", false);
_ui->statsToolBox->updateStat("Odometry/Distance/m", false);
@@ -1293,15 +1302,24 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
//Process info
_ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().inliers, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().matches, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDev/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().variance), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Variance/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().variance, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), odom.info().features<=0?0.0f:float(odom.info().matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().varianceLin), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().varianceLin, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), sqrt((float)odom.info().varianceAng), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), (float)odom.info().varianceAng, _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());
_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());
_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());
float x=0.0f,y,z, roll,pitch,yaw;
@@ -1341,6 +1359,10 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_ui->statsToolBox->updateStat("Odometry/TGroll/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), roll*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TGpitch/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), pitch*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TGyaw/deg", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), yaw*180.0/CV_PI, _preferencesDialog->isCacheSavedInFigures());
if(odom.info().interval > 0)
{
_ui->statsToolBox->updateStat("Odometry/SpeedG/kph", _preferencesDialog->isTimeUsedInFigures()?odom.data().stamp()-_firstStamp:(float)odom.data().id(), x/odom.info().interval*3.6f, _preferencesDialog->isCacheSavedInFigures());
}
}
//cumulative pose
@@ -1689,7 +1711,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
}
std::map<int, Transform> poses = stat.poses();
Transform groundTruthOffset = alignPosesToGroundTruth(poses, groundTruth);
Transform groundTruthOffset = alignPosesToGroundTruth(poses, groundTruth, stat.stamp(), stat.refImageId());
UDEBUG("time= %d ms", time.restart());
if(!_odometryReceived && poses.size())
@@ -1749,112 +1771,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
UDEBUG("time= %d ms", time.restart());
}
// ground truth live statistics
if(poses.size() && groundTruth.size())
{
std::vector<float> translationalErrors(poses.size());
std::vector<float> rotationalErrors(poses.size());
int oi=0;
float sumTranslationalErrors = 0.0f;
float sumRotationalErrors = 0.0f;
float sumSqrdTranslationalErrors = 0.0f;
float sumSqrdRotationalErrors = 0.0f;
float radToDegree = 180.0f / M_PI;
float translational_min = 0.0f;
float translational_max = 0.0f;
float rotational_min = 0.0f;
float rotational_max = 0.0f;
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
std::map<int, Transform>::iterator jter = groundTruth.find(iter->first);
if(jter!=groundTruth.end())
{
Eigen::Vector3f vA = iter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
Eigen::Vector3f vB = jter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
rotationalErrors[oi] = a*radToDegree;
translationalErrors[oi] = iter->second.getDistance(jter->second);
sumTranslationalErrors+=translationalErrors[oi];
sumSqrdTranslationalErrors+=translationalErrors[oi]*translationalErrors[oi];
sumRotationalErrors+=rotationalErrors[oi];
sumSqrdRotationalErrors+=rotationalErrors[oi]*rotationalErrors[oi];
if(oi == 0)
{
translational_min = translational_max = translationalErrors[oi];
rotational_min = rotational_max = rotationalErrors[oi];
}
else
{
if(translationalErrors[oi] < translational_min)
{
translational_min = translationalErrors[oi];
}
else if(translationalErrors[oi] > translational_max)
{
translational_max = translationalErrors[oi];
}
if(rotationalErrors[oi] < rotational_min)
{
rotational_min = rotationalErrors[oi];
}
else if(rotationalErrors[oi] > rotational_max)
{
rotational_max = rotationalErrors[oi];
}
}
++oi;
}
}
translationalErrors.resize(oi);
rotationalErrors.resize(oi);
if(oi)
{
float total = float(oi);
float translational_rmse = std::sqrt(sumSqrdTranslationalErrors/total);
float translational_mean = sumTranslationalErrors/total;
float translational_median = translationalErrors[oi/2];
float translational_std = std::sqrt(uVariance(translationalErrors, translational_mean));
float rotational_rmse = std::sqrt(sumSqrdRotationalErrors/total);
float rotational_mean = sumRotationalErrors/total;
float rotational_median = rotationalErrors[oi/2];
float rotational_std = std::sqrt(uVariance(rotationalErrors, rotational_mean));
UINFO("translational_rmse=%f", translational_rmse);
UINFO("translational_mean=%f", translational_mean);
UINFO("translational_median=%f", translational_median);
UINFO("translational_std=%f", translational_std);
UINFO("translational_min=%f", translational_min);
UINFO("translational_max=%f", translational_max);
UINFO("rotational_rmse=%f", rotational_rmse);
UINFO("rotational_mean=%f", rotational_mean);
UINFO("rotational_median=%f", rotational_median);
UINFO("rotational_std=%f", rotational_std);
UINFO("rotational_min=%f", rotational_min);
UINFO("rotational_max=%f", rotational_max);
_ui->statsToolBox->updateStat("GT/translational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_rmse, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_mean/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_median/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_median, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_std/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_min/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_min, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_max/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), translational_max, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_rmse, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_mean/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_median/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_median, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_std/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_min/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_min, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_max/", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), rotational_max, _preferencesDialog->isCacheSavedInFigures());
}
}
UDEBUG("time= %d ms", time.restart());
}
if( _ui->graphicsView_graphView->isVisible())
@@ -2990,50 +2906,176 @@ void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, i
Transform MainWindow::alignPosesToGroundTruth(
std::map<int, Transform> & poses,
const std::map<int, Transform> & groundTruth)
const std::map<int, Transform> & groundTruth,
double stamp,
int refId)
{
Transform t = Transform::getIdentity();
if(groundTruth.size() && poses.size() && _preferencesDialog->isGroundTruthAligned())
if(groundTruth.size() && poses.size())
{
unsigned int maxSize = poses.size()>groundTruth.size()? (unsigned int)poses.size(): (unsigned int)groundTruth.size();
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
cloud1.resize(maxSize);
cloud2.resize(maxSize);
int oi = 0;
int idFirst = 0;
for(std::map<int, Transform>::const_iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter)
if(_preferencesDialog->isGroundTruthAligned())
{
std::map<int, Transform>::iterator iter2 = poses.find(iter->first);
if(iter2!=poses.end())
unsigned int maxSize = poses.size()>groundTruth.size()? (unsigned int)poses.size(): (unsigned int)groundTruth.size();
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
cloud1.resize(maxSize);
cloud2.resize(maxSize);
int oi = 0;
int idFirst = 0;
for(std::map<int, Transform>::const_iterator iter=groundTruth.begin(); iter!=groundTruth.end(); ++iter)
{
if(oi==0)
std::map<int, Transform>::iterator iter2 = poses.find(iter->first);
if(iter2!=poses.end())
{
idFirst = iter->first;
if(oi==0)
{
idFirst = iter->first;
}
cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
}
cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
}
if(oi>5)
{
cloud1.resize(oi);
cloud2.resize(oi);
t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
}
else if(idFirst)
{
t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
}
if(!t.isIdentity())
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
iter->second = t * iter->second;
}
}
UDEBUG("t=%s", t.prettyPrint().c_str());
}
if(oi>5)
// ground truth live statistics
std::vector<float> translationalErrors(poses.size());
std::vector<float> rotationalErrors(poses.size());
int oi=0;
float sumTranslationalErrors = 0.0f;
float sumRotationalErrors = 0.0f;
float sumSqrdTranslationalErrors = 0.0f;
float sumSqrdRotationalErrors = 0.0f;
float radToDegree = 180.0f / M_PI;
float translational_min = 0.0f;
float translational_max = 0.0f;
float rotational_min = 0.0f;
float rotational_max = 0.0f;
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
cloud1.resize(oi);
cloud2.resize(oi);
t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
}
else if(idFirst)
{
t = groundTruth.at(idFirst) * poses.at(idFirst).inverse();
}
if(!t.isIdentity())
{
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
std::map<int, Transform>::const_iterator jter = groundTruth.find(iter->first);
if(jter!=groundTruth.end())
{
iter->second = t * iter->second;
Eigen::Vector3f vA = iter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
Eigen::Vector3f vB = jter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
double a = pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
rotationalErrors[oi] = a*radToDegree;
translationalErrors[oi] = iter->second.getDistance(jter->second);
sumTranslationalErrors+=translationalErrors[oi];
sumSqrdTranslationalErrors+=translationalErrors[oi]*translationalErrors[oi];
sumRotationalErrors+=rotationalErrors[oi];
sumSqrdRotationalErrors+=rotationalErrors[oi]*rotationalErrors[oi];
if(oi == 0)
{
translational_min = translational_max = translationalErrors[oi];
rotational_min = rotational_max = rotationalErrors[oi];
}
else
{
if(translationalErrors[oi] < translational_min)
{
translational_min = translationalErrors[oi];
}
else if(translationalErrors[oi] > translational_max)
{
translational_max = translationalErrors[oi];
}
if(rotationalErrors[oi] < rotational_min)
{
rotational_min = rotationalErrors[oi];
}
else if(rotationalErrors[oi] > rotational_max)
{
rotational_max = rotationalErrors[oi];
}
}
++oi;
}
}
translationalErrors.resize(oi);
rotationalErrors.resize(oi);
if(oi)
{
float total = float(oi);
float translational_rmse = std::sqrt(sumSqrdTranslationalErrors/total);
float translational_mean = sumTranslationalErrors/total;
float translational_median = translationalErrors[oi/2];
float translational_std = std::sqrt(uVariance(translationalErrors, translational_mean));
float rotational_rmse = std::sqrt(sumSqrdRotationalErrors/total);
float rotational_mean = sumRotationalErrors/total;
float rotational_median = rotationalErrors[oi/2];
float rotational_std = std::sqrt(uVariance(rotationalErrors, rotational_mean));
UINFO("translational_rmse=%f", translational_rmse);
UINFO("translational_mean=%f", translational_mean);
UINFO("translational_median=%f", translational_median);
UINFO("translational_std=%f", translational_std);
UINFO("translational_min=%f", translational_min);
UINFO("translational_max=%f", translational_max);
UINFO("rotational_rmse=%f", rotational_rmse);
UINFO("rotational_mean=%f", rotational_mean);
UINFO("rotational_median=%f", rotational_median);
UINFO("rotational_std=%f", rotational_std);
UINFO("rotational_min=%f", rotational_min);
UINFO("rotational_max=%f", rotational_max);
if((_preferencesDialog->isTimeUsedInFigures() && stamp > 0.0) || (refId && refId>=0))
{
_ui->statsToolBox->updateStat("GT/translational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_rmse, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_median, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_min, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, translational_max, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_rmse/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_rmse, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_mean/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_median/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_median, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_std/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_min/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_min, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_max/", _preferencesDialog->isTimeUsedInFigures()?stamp-_firstStamp:refId, rotational_max, _preferencesDialog->isCacheSavedInFigures());
}
else
{
_ui->statsToolBox->updateStat("GT/translational_rmse/", translational_rmse, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_mean/", translational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_median/", translational_median, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_std/", translational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_min/", translational_min, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/translational_max/", translational_max, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_rmse/", rotational_rmse, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_mean/", rotational_mean, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_median/", rotational_median, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_std/", rotational_std, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_min/", rotational_min, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("GT/rotational_max/", rotational_max, _preferencesDialog->isCacheSavedInFigures());
}
}
UDEBUG("t=%s", t.prettyPrint().c_str());
}
return t;
}
@@ -4191,7 +4233,7 @@ void MainWindow::startDetection()
_preferencesDialog->getSourceScanFromDepthMaxDepth(),
_preferencesDialog->getSourceScanVoxelSize(),
_preferencesDialog->getSourceScanNormalsK());
if(_preferencesDialog->getSourceType() == PreferencesDialog::kSrcRGBD)
if(_preferencesDialog->isDepthFilteringAvailable())
{
if(_preferencesDialog->isBilateralFiltering())
{
@@ -4234,7 +4276,12 @@ void MainWindow::startDetection()
if(!camera->odomProvided() && !_preferencesDialog->isOdomDisabled())
{
Odometry * odom = Odometry::create(parameters);
ParametersMap odomParameters = parameters;
if(_preferencesDialog->getOdomRegistrationApproach() < 3)
{
uInsert(odomParameters, ParametersPair(Parameters::kRegStrategy(), uNumber2Str(_preferencesDialog->getOdomRegistrationApproach())));
}
Odometry * odom = Odometry::create(odomParameters);
_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());
UEventsManager::addHandler(_odomThread);
@@ -4537,7 +4584,6 @@ void MainWindow::postProcessing()
int detectLoopClosureIterations = _postProcessingDialog->iterations();
bool sba = _postProcessingDialog->isSBA();
int sbaIterations = _postProcessingDialog->sbaIterations();
double sbaEpsilon = _postProcessingDialog->sbaEpsilon();
double sbaVariance = _postProcessingDialog->sbaVariance();
Optimizer::Type sbaType = _postProcessingDialog->sbaType();
@@ -4689,7 +4735,7 @@ void MainWindow::postProcessing()
UINFO("Added new loop closure between %d and %d.", from, to);
addedLinks.insert(from);
addedLinks.insert(to);
_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.variance, info.variance)));
_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.varianceAng, info.varianceLin)));
++loopClosuresAdded;
_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
QApplication::processEvents();
@@ -4785,7 +4831,7 @@ void MainWindow::postProcessing()
if(!transform.isNull())
{
Link newLink(from, to, iter->second.type(), transform, info.variance, info.variance);
Link newLink(from, to, iter->second.type(), transform, info.varianceAng, info.varianceLin);
iter->second = newLink;
}
else
@@ -4846,7 +4892,6 @@ void MainWindow::postProcessing()
ParametersMap parametersSBA = _preferencesDialog->getAllParameters();
uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations)));
uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerEpsilon(), uNumber2Str(sbaEpsilon)));
uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(sbaVariance)));
Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
std::map<int, Transform> newPoses = sba->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap());
@@ -6272,7 +6317,7 @@ void MainWindow::changeState(MainWindow::State newState)
_ui->actionPause_when_a_loop_hypothesis_is_rejected->setEnabled(true);
_ui->actionDump_the_memory->setEnabled(true);
_ui->actionDump_the_prediction_matrix->setEnabled(true);
_ui->actionDelete_memory->setEnabled(true);
_ui->actionDelete_memory->setEnabled(_openedDatabasePath.isEmpty());
_ui->actionPost_processing->setEnabled(_cachedSignatures.size() >= 2 && _currentPosesMap.size() >= 2 && _currentLinksMap.size() >= 1);
_ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
_ui->actionGenerate_map->setEnabled(true);

16
guilib/src/PostProcessingDialog.cpp
View File

@@ -72,7 +72,6 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
connect(_ui->sba, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
connect(_ui->sba_iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->sba_epsilon, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateVisibility()));
@@ -104,7 +103,6 @@ void PostProcessingDialog::saveSettings(QSettings & settings, const QString & gr
settings.setValue("refine_lc", this->isRefineLoopClosureLinks());
settings.setValue("sba", this->isSBA());
settings.setValue("sba_iterations", this->sbaIterations());
settings.setValue("sba_epsilon", this->sbaEpsilon());
settings.setValue("sba_type", this->sbaType());
settings.setValue("sba_variance", this->sbaVariance());
if(!group.isEmpty())
@@ -127,7 +125,6 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr
this->setRefineLoopClosureLinks(settings.value("refine_lc", this->isRefineLoopClosureLinks()).toBool());
this->setSBA(settings.value("sba", this->isSBA()).toBool());
this->setSBAIterations(settings.value("sba_iterations", this->sbaIterations()).toInt());
this->setSBAEpsilon(settings.value("sba_epsilon", this->sbaEpsilon()).toDouble());
this->setSBAType((Optimizer::Type)settings.value("sba_type", this->sbaType()).toInt());
this->setSBAVariance(settings.value("sba_variance", this->sbaVariance()).toDouble());
if(!group.isEmpty())
@@ -139,14 +136,13 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr
void PostProcessingDialog::restoreDefaults()
{
setDetectMoreLoopClosures(true);
setClusterRadius(0.5);
setClusterRadius(1);
setClusterAngle(30);
setIterations(3);
setIterations(5);
setRefineNeighborLinks(false);
setRefineLoopClosureLinks(false);
setSBA(false);
setSBAIterations(20);
setSBAEpsilon(0.0);
setSBAType(!Optimizer::isAvailable(Optimizer::kTypeG2O)&&Optimizer::isAvailable(Optimizer::kTypeCVSBA)?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O);
setSBAVariance(1.0);
}
@@ -196,10 +192,6 @@ int PostProcessingDialog::sbaIterations() const
{
return _ui->sba_iterations->value();
}
double PostProcessingDialog::sbaEpsilon() const
{
return _ui->sba_epsilon->value();
}
double PostProcessingDialog::sbaVariance() const
{
return _ui->sba_variance->value();
@@ -242,10 +234,6 @@ void PostProcessingDialog::setSBAIterations(int iterations)
{
_ui->sba_iterations->setValue(iterations);
}
void PostProcessingDialog::setSBAEpsilon(double epsilon)
{
_ui->sba_epsilon->setValue(epsilon);
}
void PostProcessingDialog::setSBAVariance(double variance)
{
_ui->sba_variance->setValue(variance);

2
guilib/src/PostProcessingDialog.h
View File

@@ -59,7 +59,6 @@ public:
bool isRefineLoopClosureLinks() const;
bool isSBA() const;
int sbaIterations() const;
double sbaEpsilon() const;
double sbaVariance() const;
Optimizer::Type sbaType() const;
@@ -72,7 +71,6 @@ public:
void setRefineLoopClosureLinks(bool on);
void setSBA(bool on);
void setSBAIterations(int iterations);
void setSBAEpsilon(double epsilon);
void setSBAVariance(double variance);
void setSBAType(Optimizer::Type type);

80
guilib/src/PreferencesDialog.cpp
View File

@@ -180,6 +180,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->groupBox_fast_opencv2->setEnabled(false);
#endif
_ui->comboBox_cameraImages_odomFormat->setItemData(4, 0, Qt::UserRole - 1);
_ui->comboBox_cameraImages_gtFormat->setItemData(4, 0, Qt::UserRole - 1);
if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
@@ -255,6 +256,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable());
_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
// in case we change the ui, we should not forget to change stuff related to this parameter
UASSERT(_ui->odom_registration->count() == 4);
// Default Driver
connect(_ui->comboBox_sourceType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
@@ -291,6 +295,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->spinBox_odomQualityWarnThr, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
connect(_ui->checkBox_posteriorGraphView, SIGNAL(stateChanged(int)), 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->checkbox_groundTruthAlign, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
// Cloud rendering panel
@@ -474,6 +479,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->toolButton_cameraRGBDImages_path_rgb, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesPathRGB()));
connect(_ui->toolButton_cameraRGBDImages_path_depth, SIGNAL(clicked()), this, SLOT(selectSourceRGBDImagesPathDepth()));
connect(_ui->toolButton_cameraImages_path_scans, SIGNAL(clicked()), this, SLOT(selectSourceImagesPathScans()));
connect(_ui->toolButton_cameraImages_odom, SIGNAL(clicked()), this, SLOT(selectSourceImagesPathOdom()));
connect(_ui->toolButton_cameraImages_gt, SIGNAL(clicked()), this, SLOT(selectSourceImagesPathGt()));
connect(_ui->lineEdit_cameraRGBDImages_path_rgb, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_cameraRGBDImages_path_depth, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -485,6 +491,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->spinBox_cameraImages_max_scan_pts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_cameraImages_scanDownsampleStep, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->doubleSpinBox_cameraImages_scanVoxelSize, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_cameraImages_odom, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_cameraImages_odomFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->lineEdit_cameraImages_gt, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_cameraImages_gtFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->groupBox_depthFromScan, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
@@ -726,6 +734,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->comboBox_g2o_solver->setObjectName(Parameters::kg2oSolver().c_str());
_ui->comboBox_g2o_optimizer->setObjectName(Parameters::kg2oOptimizer().c_str());
_ui->doubleSpinBox_g2o_pixelVariance->setObjectName(Parameters::kg2oPixelVariance().c_str());
_ui->doubleSpinBox_g2o_robustKernelDelta->setObjectName(Parameters::kg2oRobustKernelDelta().c_str());
_ui->doubleSpinBox_g2o_baseline->setObjectName(Parameters::kg2oBaseline().c_str());
_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());
@@ -833,6 +843,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
_ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str());
_ui->odom_flow_keyframeThr->setObjectName(Parameters::kOdomKeyFrameThr().c_str());
_ui->odom_VisKeyFrameThr->setObjectName(Parameters::kOdomVisKeyFrameThr().c_str());
_ui->odom_flow_scanKeyframeThr->setObjectName(Parameters::kOdomScanKeyFrameThr().c_str());
_ui->odom_flow_guessMotion->setObjectName(Parameters::kOdomGuessMotion().c_str());
_ui->odom_imageDecimation->setObjectName(Parameters::kOdomImageDecimation().c_str());
@@ -1395,6 +1406,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->spinBox_cameraImages_max_scan_pts->setValue(0);
_ui->spinBox_cameraImages_scanDownsampleStep->setValue(1);
_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(0.0f);
_ui->lineEdit_cameraImages_odom->setText("");
_ui->comboBox_cameraImages_odomFormat->setCurrentIndex(0);
_ui->lineEdit_cameraImages_gt->setText("");
_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(0);
@@ -1477,6 +1490,11 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
{
this->setupKpRoiPanel();
}
if(groupBox->objectName() == _ui->groupBox_odometry1->objectName())
{
_ui->odom_registration->setCurrentIndex(3);
}
}
}
@@ -1596,6 +1614,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
_ui->spinBox_odomQualityWarnThr->setValue(settings.value("odomQualityThr", _ui->spinBox_odomQualityWarnThr->value()).toInt());
_ui->checkBox_posteriorGraphView->setChecked(settings.value("posteriorGraphView", _ui->checkBox_posteriorGraphView->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->checkbox_groundTruthAlign->setChecked(settings.value("gtAlign", _ui->checkbox_groundTruthAlign->isChecked()).toBool());
for(int i=0; i<2; ++i)
@@ -1766,6 +1785,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->spinBox_cameraImages_max_scan_pts->setValue(settings.value("scan_max_pts", _ui->spinBox_cameraImages_max_scan_pts->value()).toInt());
_ui->spinBox_cameraImages_scanDownsampleStep->setValue(settings.value("scan_downsample_step", _ui->spinBox_cameraImages_scanDownsampleStep->value()).toInt());
_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(settings.value("scan_voxel_size", _ui->doubleSpinBox_cameraImages_scanVoxelSize->value()).toDouble());
_ui->lineEdit_cameraImages_odom->setText(settings.value("odom_path", _ui->lineEdit_cameraImages_odom->text()).toString());
_ui->comboBox_cameraImages_odomFormat->setCurrentIndex(settings.value("odom_format", _ui->comboBox_cameraImages_odomFormat->currentIndex()).toInt());
_ui->lineEdit_cameraImages_gt->setText(settings.value("gt_path", _ui->lineEdit_cameraImages_gt->text()).toString());
_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(settings.value("gt_format", _ui->comboBox_cameraImages_gtFormat->currentIndex()).toInt());
settings.endGroup(); // images
@@ -1975,6 +1996,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
settings.setValue("odomQualityThr", _ui->spinBox_odomQualityWarnThr->value());
settings.setValue("posteriorGraphView", _ui->checkBox_posteriorGraphView->isChecked());
settings.setValue("odomDisabled", _ui->checkbox_odomDisabled->isChecked());
settings.setValue("odomRegistration", _ui->odom_registration->currentIndex());
settings.setValue("gtAlign", _ui->checkbox_groundTruthAlign->isChecked());
for(int i=0; i<2; ++i)
@@ -2147,6 +2169,8 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("scan_max_pts", _ui->spinBox_cameraImages_max_scan_pts->value());
settings.setValue("scan_downsample_step", _ui->spinBox_cameraImages_scanDownsampleStep->value());
settings.setValue("scan_voxel_size", _ui->doubleSpinBox_cameraImages_scanVoxelSize->value());
settings.setValue("odom_path", _ui->lineEdit_cameraImages_odom->text());
settings.setValue("odom_format", _ui->comboBox_cameraImages_odomFormat->currentIndex());
settings.setValue("gt_path", _ui->lineEdit_cameraImages_gt->text());
settings.setValue("gt_format", _ui->comboBox_cameraImages_gtFormat->currentIndex());
settings.endGroup(); // images
@@ -2305,7 +2329,9 @@ bool PreferencesDialog::validateForm()
"with cvsba. Bundle adjustment is disabled."));
_ui->odom_f2m_bundleStrategy->setCurrentIndex(0);
}
if(_ui->odom_f2m_bundleStrategy->currentIndex() > 0 && _ui->loopClosure_correspondencesType->currentIndex() == 1)
if(_ui->odom_strategy->currentIndex() == 0 && // F2M
_ui->odom_f2m_bundleStrategy->currentIndex() > 0 &&
_ui->loopClosure_correspondencesType->currentIndex() == 1)
{
QMessageBox::warning(this, tr("Parameter warning"),
tr("Odometry local bundle adjustment optimization cannot be used at the same time than Optical Flow correspondences "
@@ -2916,6 +2942,30 @@ void PreferencesDialog::selectSourceRGBDImagesPathDepth()
}
}
void PreferencesDialog::selectSourceImagesPathOdom()
{
QString dir = _ui->lineEdit_cameraImages_odom->text();
if(dir.isEmpty())
{
dir = getWorkingDirectory();
}
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Odometry (*.txt *.log *.toro *.g2o)"));
if(path.size())
{
QStringList list;
for(int i=0; i<_ui->comboBox_cameraImages_odomFormat->count(); ++i)
{
list.push_back(_ui->comboBox_cameraImages_odomFormat->itemText(i));
}
QString item = QInputDialog::getItem(this, tr("Odometry Format"), tr("Format:"), list, _ui->comboBox_cameraImages_odomFormat->currentIndex(), false);
if(!item.isEmpty())
{
_ui->lineEdit_cameraImages_odom->setText(path);
_ui->comboBox_cameraImages_odomFormat->setCurrentIndex(_ui->comboBox_cameraImages_odomFormat->findText(item));
}
}
}
void PreferencesDialog::selectSourceImagesPathGt()
{
QString dir = _ui->lineEdit_cameraImages_gt->text();
@@ -2931,7 +2981,7 @@ void PreferencesDialog::selectSourceImagesPathGt()
{
list.push_back(_ui->comboBox_cameraImages_gtFormat->itemText(i));
}
QString item = QInputDialog::getItem(this, tr("Ground Truth Format"), tr("Format:"), list, 0, false);
QString item = QInputDialog::getItem(this, tr("Ground Truth Format"), tr("Format:"), list, _ui->comboBox_cameraImages_gtFormat->currentIndex(), false);
if(!item.isEmpty())
{
_ui->lineEdit_cameraImages_gt->setText(path);
@@ -3704,6 +3754,11 @@ void PreferencesDialog::updateSourceGrpVisibility()
(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo) ||
(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB));
_ui->groupBox_depthImageFiltering->setEnabled(
_ui->comboBox_sourceType->currentIndex() == 0 || // RGBD
_ui->comboBox_sourceType->currentIndex() == 3); // Database
_ui->groupBox_depthImageFiltering->setVisible(_ui->groupBox_depthImageFiltering->isEnabled());
//_ui->groupBox_scan->setVisible(_ui->comboBox_sourceType->currentIndex() != 3);
_ui->groupBox_depthFromScan->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB);
@@ -3787,6 +3842,10 @@ bool PreferencesDialog::isOdomDisabled() const
{
return _ui->checkbox_odomDisabled->isChecked();
}
int PreferencesDialog::getOdomRegistrationApproach() const
{
return _ui->odom_registration->currentIndex();
}
bool PreferencesDialog::isGroundTruthAligned() const
{
return _ui->checkbox_groundTruthAlign->isChecked();
@@ -4135,6 +4194,10 @@ bool PreferencesDialog::isSourceRGBDColorOnly() const
{
return _ui->checkbox_rgbd_colorOnly->isChecked();
}
bool PreferencesDialog::isDepthFilteringAvailable() const
{
return _ui->groupBox_depthImageFiltering->isEnabled();
}
QString PreferencesDialog::getSourceDistortionModel() const
{
return _ui->lineEdit_source_distortionModel->text();
@@ -4277,6 +4340,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
this->getGeneralInputRate(),
this->getSourceLocalTransform());
((CameraRGBDImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
((CameraRGBDImages*)camera)->setOdometryPath(_ui->lineEdit_cameraImages_odom->text().toStdString(), _ui->comboBox_cameraImages_odomFormat->currentIndex());
((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
((CameraRGBDImages*)camera)->setScanPath(
_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
@@ -4321,6 +4385,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
this->getGeneralInputRate(),
this->getSourceLocalTransform());
((CameraStereoImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
((CameraStereoImages*)camera)->setOdometryPath(_ui->lineEdit_cameraImages_odom->text().toStdString(), _ui->comboBox_cameraImages_odomFormat->currentIndex());
((CameraStereoImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
((CameraStereoImages*)camera)->setScanPath(
_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
@@ -4421,6 +4486,9 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
((CameraImages*)camera)->setImagesRectified(_ui->checkBox_rgbImages_rectify->isChecked() && !useRawImages);
((CameraImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
((CameraImages*)camera)->setOdometryPath(
_ui->lineEdit_cameraImages_odom->text().toStdString(),
_ui->comboBox_cameraImages_odomFormat->currentIndex());
((CameraImages*)camera)->setGroundTruthPath(
_ui->lineEdit_cameraImages_gt->text().toStdString(),
_ui->comboBox_cameraImages_gtFormat->currentIndex());
@@ -4643,6 +4711,10 @@ void PreferencesDialog::testOdometry()
}
ParametersMap parameters = this->getAllParameters();
if(getOdomRegistrationApproach() < 3)
{
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), uNumber2Str(getOdomRegistrationApproach())));
}
Odometry * odometry = Odometry::create(parameters);
OdometryThread odomThread(
@@ -4672,7 +4744,7 @@ void PreferencesDialog::testOdometry()
_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
_ui->spinBox_cameraImages_scanNormalsK->value());
if(this->getSourceType() == PreferencesDialog::kSrcRGBD)
if(isDepthFilteringAvailable())
{
if(_ui->groupBox_bilateral->isChecked())
{
@@ -4720,7 +4792,7 @@ void PreferencesDialog::testCamera()
_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
_ui->spinBox_cameraImages_scanNormalsK->value());
if(this->getSourceType() == PreferencesDialog::kSrcRGBD)
if(isDepthFilteringAvailable())
{
if(_ui->groupBox_bilateral->isChecked())
{

30
guilib/src/ui/DatabaseViewer.ui
View File

@@ -52,8 +52,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>194</width>
<height>196</height>
<width>253</width>
<height>208</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -210,8 +210,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>194</width>
<height>196</height>
<width>253</width>
<height>208</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -484,7 +484,7 @@
<x>0</x>
<y>0</y>
<width>1285</width>
<height>25</height>
<height>22</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -994,8 +994,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>424</width>
<height>186</height>
<width>324</width>
<height>188</height>
</rect>
</property>
<attribute name="label">
@@ -1130,8 +1130,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>424</width>
<height>535</height>
<width>280</width>
<height>584</height>
</rect>
</property>
<attribute name="label">
@@ -1447,8 +1447,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>437</width>
<height>162</height>
<width>201</width>
<height>126</height>
</rect>
</property>
<attribute name="label">
@@ -1547,8 +1547,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>424</width>
<height>198</height>
<width>333</width>
<height>256</height>
</rect>
</property>
<attribute name="label">
@@ -2014,12 +2014,12 @@
</action>
<action name="actionView_3D_laser_scans">
<property name="text">
<string>View 3D laser scans...</string>
<string>View 2D-3D laser scans...</string>
</property>
</action>
<action name="actionExport_3D_laser_scans_ply_pcd">
<property name="text">
<string>Export 3D laser scans (*.ply *.pcd) ...</string>
<string>Export 2D-3D laser scans (*.ply *.pcd) ...</string>
</property>
</action>
<action name="actionRegenerate_local_grid_maps">

2
guilib/src/ui/mainWindow.ui
View File

@@ -1141,7 +1141,7 @@
</action>
<action name="actionExport_2D_scans_ply_pcd">
<property name="text">
<string>Export 2D scans (*.ply *.pcd)...</string>
<string>Export 2D-3D scans (*.ply *.pcd)...</string>
</property>
</action>
<action name="actionView_scans">

104
guilib/src/ui/postProcessingDialog.ui
View File

@@ -194,42 +194,6 @@
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QComboBox" name="comboBox_sbaType">
<item>
<property name="text">
<string>g2o</string>
</property>
</item>
<item>
<property name="text">
<string>cvsba</string>
</property>
</item>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="sba_epsilon">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>4</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.000100000000000</double>
</property>
<property name="value">
<double>0.000100000000000</double>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_8">
<property name="text">
@@ -241,15 +205,37 @@
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_9">
<widget class="QLabel" name="label_variance">
<property name="text">
<string>Stop optimizing when the error improvement is less than this value.</string>
<string>Pixel variance used by g2o.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="sba_variance">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.010000000000000</double>
</property>
<property name="maximum">
<double>999.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_10">
<property name="text">
@@ -260,36 +246,18 @@
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_variance">
<property name="text">
<string>Pixel variance used by g2o.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="sba_variance">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>4</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>10.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
<item row="0" column="0">
<widget class="QComboBox" name="comboBox_sbaType">
<item>
<property name="text">
<string>g2o</string>
</property>
</item>
<item>
<property name="text">
<string>cvsba</string>
</property>
</item>
</widget>
</item>
</layout>

1011
guilib/src/ui/preferencesDialog.ui
View File

File diff suppressed because it is too large Load Diff