diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index b0341254..a8d92d11 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -181,7 +181,7 @@ public: std::multimap & links, bool lookInDatabase = false); - Transform computeTransform(int fromId, int toId, RegistrationInfo * info = 0); + Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0); Transform computeIcpTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0); Transform computeIcpTransformMulti( int newId, diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index e00be6bd..e990816e 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -326,6 +326,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(RGBD, ProximityMaxGraphDepth, int, 50, "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore."); RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 0.5, "Path filtering radius."); RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed, bool, true, "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph."); + RTABMAP_PARAM(RGBD, ProximityAngle, float, 45.0, "Maximum angle (degrees) for visual proximity detection."); // Graph optimization #ifdef RTABMAP_GTSAM @@ -358,7 +359,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Odom, ParticleLambdaR, float, 100, "Lambda of rotational components (roll,pitch,yaw)."); RTABMAP_PARAM(Odom, KalmanProcessNoise, float, 0.001, "Process noise covariance value."); RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01, "Process measurement covariance value."); - RTABMAP_PARAM(Odom, GuessMotion, bool, false, "Guess next transformation from the last motion computed."); + RTABMAP_PARAM(Odom, GuessMotion, bool, true, "Guess next transformation from the last motion computed."); RTABMAP_PARAM(Odom, KeyFrameThr, float, 0.5, "Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame."); // Odometry Bag-of-words @@ -399,7 +400,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Vis, CorType, int, 0, "Correspondences computation approach: 0=Features Matching, 1=Optical Flow"); RTABMAP_PARAM(Vis, CorNNType, int, 3, "[Vis/CorrespondenceType=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach."); RTABMAP_PARAM(Vis, CorNNDR, float, 0.8, "[Vis/CorrespondenceType=0] NNDR: nearest neighbor distance ratio. Used for features matching approach."); - RTABMAP_PARAM(Vis, CorGuessWinSize, int, 16, "[Vis/CorrespondenceType=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled."); + RTABMAP_PARAM(Vis, CorGuessWinSize, int, 50, "[Vis/CorrespondenceType=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled."); RTABMAP_PARAM(Vis, CorFlowWinSize, int, 16, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach."); RTABMAP_PARAM(Vis, CorFlowIterations, int, 30, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach."); RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach."); diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index 174db0f0..c4c65ad9 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -195,6 +195,7 @@ private: float _proximityFilteringRadius; bool _proximityRawPosesUsed; bool _proximityScansMerged; + float _proximityAngle; std::string _databasePath; bool _optimizeFromGraphEnd; float _optimizationMaxLinearError; diff --git a/corelib/src/CameraRGB.cpp b/corelib/src/CameraRGB.cpp index 9a67d7e8..4e75dfc0 100644 --- a/corelib/src/CameraRGB.cpp +++ b/corelib/src/CameraRGB.cpp @@ -125,7 +125,6 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string _count = 0; _countScan = 0; _captureDelay = 0.0; - _captureTimer.restart(); UDEBUG(""); if(_dir) @@ -404,6 +403,8 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string } } + _captureTimer.restart(); + return success; } diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 10de7259..ef3fd3fd 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -2047,6 +2047,7 @@ void Memory::removeRawData(int id) Transform Memory::computeTransform( int fromId, int toId, + Transform guess, RegistrationInfo * info) { const Signature * fromS = this->getSignature(fromId); @@ -2084,13 +2085,17 @@ Transform Memory::computeTransform( tmpTo.sensorData().setFeatures(std::vector(), cv::Mat()); } - Transform guess = Transform::getIdentity(); if(!_registrationPipeline->isImageRequired()) { // no visual in the pipeline, make visual registration for guess RegistrationVis regVis(parameters_); guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info); } + else if(guess.isNull()) + { + guess.setIdentity(); + } + if(!guess.isNull()) { transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info); diff --git a/corelib/src/RegistrationVis.cpp b/corelib/src/RegistrationVis.cpp index 55cc38e2..ff3105dd 100644 --- a/corelib/src/RegistrationVis.cpp +++ b/corelib/src/RegistrationVis.cpp @@ -288,7 +288,9 @@ Transform RegistrationVis::computeTransformationImpl( std::multimap words3To; std::multimap wordsDescFrom; std::multimap wordsDescTo; - if(_correspondencesApproach == 1) //Optical Flow + if(_correspondencesApproach == 1 && //Optical Flow + !fromSignature.sensorData().imageRaw().empty() && + !toSignature.sensorData().imageRaw().empty()) { UDEBUG(""); // convert to grayscale @@ -1054,6 +1056,11 @@ Transform RegistrationVis::computeTransformationImpl( matchesCount = (int)matches[0].size(); } } + else if(toSignature.sensorData().isValid()) + { + UWARN("Missing correspondences for registration. toWords = %d toImageEmpty=%d", + (int)toSignature.getWords().size(), toSignature.sensorData().imageRaw().empty()?1:0); + } info.inliers = inliersCount; info.matches = matchesCount; diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 6fb48e29..4edf7b19 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -101,6 +101,7 @@ Rtabmap::Rtabmap() : _proximityFilteringRadius(Parameters::defaultRGBDProximityPathFilteringRadius()), _proximityRawPosesUsed(Parameters::defaultRGBDProximityPathRawPosesUsed()), _proximityScansMerged(Parameters::defaultRGBDProximityPathScansMerged()), + _proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f), _databasePath(""), _optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()), _optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()), @@ -409,6 +410,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), _proximityFilteringRadius); Parameters::parse(parameters, Parameters::kRGBDProximityPathRawPosesUsed(), _proximityRawPosesUsed); Parameters::parse(parameters, Parameters::kRGBDProximityPathScansMerged(), _proximityScansMerged); + Parameters::parse(parameters, Parameters::kRGBDProximityAngle(), _proximityAngle); + _proximityAngle *= M_PI/180.0f; Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd); Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError); Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure); @@ -1169,7 +1172,13 @@ bool Rtabmap::process( std::string rejectedMsg; UDEBUG("Check local transform between %d and %d", signature->id(), *iter); RegistrationInfo info; - Transform transform = _memory->computeTransform(signature->id(), *iter, &info); + Transform guess; + if(_optimizedPoses.find(*iter) != _optimizedPoses.end()) + { + guess = newPose.inverse() * _optimizedPoses.at(*iter); + } + + Transform transform = _memory->computeTransform(signature->id(), *iter, guess, &info); if(!transform.isNull()) { @@ -1722,7 +1731,7 @@ bool Rtabmap::process( info.variance = 1.0f; if(_rgbdSlamMode) { - transform = _memory->computeTransform(signature->id(), _loopClosureHypothesis.first, &info); + transform = _memory->computeTransform(signature->id(), _loopClosureHypothesis.first, Transform(), &info); loopClosureVisualInliers = info.inliers; rejectedHypothesis = transform.isNull(); if(rejectedHypothesis) @@ -1814,7 +1823,7 @@ bool Rtabmap::process( //find the nearest pose on the path looking in the same direction path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id()))); - path = graph::getPosesInRadius(signature->id(), path, _localRadius, M_PI/4); + path = graph::getPosesInRadius(signature->id(), path, _localRadius, _proximityAngle); int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id())); if(nearestId > 0) { @@ -1824,7 +1833,7 @@ bool Rtabmap::process( _optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius)) { RegistrationInfo info; - Transform transform = _memory->computeTransform(signature->id(), nearestId, &info); + Transform transform = _memory->computeTransform(signature->id(), nearestId, Transform(), &info); if(!transform.isNull()) { if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius) diff --git a/corelib/src/util3d_motion_estimation.cpp b/corelib/src/util3d_motion_estimation.cpp index 9099b526..45d14eab 100644 --- a/corelib/src/util3d_motion_estimation.cpp +++ b/corelib/src/util3d_motion_estimation.cpp @@ -92,6 +92,9 @@ Transform estimateMotion3DTo2D( imagePoints.resize(oi); matches.resize(oi); + UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d", + (int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size()); + if((int)matches.size() >= minInliers) { //PnPRansac @@ -341,9 +344,7 @@ void solvePnPRansac( float inlierThreshold = reprojectionError; if(inliers.size() >= 4 && refineIterations>0) { - float inlier_distance_threshold_sqr = inlierThreshold * inlierThreshold; float error_threshold = inlierThreshold; - float sigma_sqr = refineSigma * refineSigma; int refine_iterations = 0; bool inlier_changed = false, oscillating = false; std::vector new_inliers, prev_inliers = inliers; @@ -393,7 +394,7 @@ void solvePnPRansac( // Estimate the variance and the new threshold float m = uMean(err.data(), err.size()); float variance = uVariance(err.data(), err.size()); - error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance)); + error_threshold = std::min(inlierThreshold, refineSigma * float(sqrt(variance))); UDEBUG ("RANSAC refineModel: New estimated error threshold: %f (variance=%f mean=%f) on iteration %d out of %d.", error_threshold, variance, m, refine_iterations, refineIterations); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index a2a475f6..fe1550f6 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -483,6 +483,44 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _ui->statsToolBox->updateStat("Planning/Goal/", 0.0f); _ui->statsToolBox->updateStat("Planning/Poses/", 0.0f); _ui->statsToolBox->updateStat("Planning/Length/m", 0.0f); + + _ui->statsToolBox->updateStat("Camera/Time capturing/ms", 0.0f); + _ui->statsToolBox->updateStat("Camera/Time decimation/ms", 0.0f); + _ui->statsToolBox->updateStat("Camera/Time disparity/ms", 0.0f); + _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", 0.0f); + _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", 0.0f); + + _ui->statsToolBox->updateStat("Odometry/ID/", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Features/", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Matches/", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Inliers/", 0.0f); + _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", 0.0f); + _ui->statsToolBox->updateStat("Odometry/StdDev/", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Variance/", 0.0f); + _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", 0.0f); + _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", 0.0f); + _ui->statsToolBox->updateStat("Odometry/LocalMapSize/", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Interval/ms", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Speed/kph", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Distance/m", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Tx/m", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Ty/m", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Tz/m", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Troll/deg", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Tpitch/deg", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Tyaw/deg", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Px/m", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Py/m", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Pz/m", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Proll/deg", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Ppitch/deg", 0.0f); + _ui->statsToolBox->updateStat("Odometry/Pyaw/deg", 0.0f); + + _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", 0.0f); + _ui->statsToolBox->updateStat("GUI/RGB-D cloud/ms", 0.0f); + _ui->statsToolBox->updateStat("GUI/RGB-D closure_view/ms", 0.0f); + _ui->statsToolBox->updateStat("GUI/Refresh stats/ms", 0.0f); + this->loadFigures(); connect(_ui->statsToolBox, SIGNAL(figuresSetupChanged()), this, SLOT(configGUIModified())); @@ -735,11 +773,11 @@ void MainWindow::handleEvent(UEvent* anEvent) void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info) { - _ui->statsToolBox->updateStat("Camera/Time capturing/ms", (float)info.id, (float)info.timeCapture*1000.0); - _ui->statsToolBox->updateStat("Camera/Time decimation/ms", (float)info.id, (float)info.timeImageDecimation*1000.0); - _ui->statsToolBox->updateStat("Camera/Time disparity/ms", (float)info.id, (float)info.timeDisparity*1000.0); - _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", (float)info.id, (float)info.timeMirroring*1000.0); - _ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", (float)info.id, (float)info.timeScanFromDepth*1000.0); + _ui->statsToolBox->updateStat("Camera/Time capturing/ms", (float)info.id, info.timeCapture*1000.0f); + _ui->statsToolBox->updateStat("Camera/Time decimation/ms", (float)info.id, info.timeImageDecimation*1000.0f); + _ui->statsToolBox->updateStat("Camera/Time disparity/ms", (float)info.id, info.timeDisparity*1000.0f); + _ui->statsToolBox->updateStat("Camera/Time mirroring/ms", (float)info.id, info.timeMirroring*1000.0f); + _ui->statsToolBox->updateStat("Camera/Time scan_from_depth/ms", (float)info.id, info.timeScanFromDepth*1000.0f); } void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom) @@ -1060,7 +1098,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom) } if(odom.info().icpInliersRatio >= 0) { - _ui->statsToolBox->updateStat("Odometry/ICP_Inliers_Ratio/", (float)odom.data().id(), (float)odom.info().icpInliersRatio); + _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", (float)odom.data().id(), (float)odom.info().icpInliersRatio); } if(odom.info().matches >= 0) { @@ -1076,11 +1114,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom) } if(odom.info().timeEstimation > 0) { - _ui->statsToolBox->updateStat("Odometry/Time_Estimation/ms", (float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f); + _ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", (float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f); } if(odom.info().timeParticleFiltering > 0) { - _ui->statsToolBox->updateStat("Odometry/Time_Filtering/ms", (float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f); + _ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", (float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f); } if(odom.info().features >=0) { @@ -1088,7 +1126,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom) } if(odom.info().localMapSize >=0) { - _ui->statsToolBox->updateStat("Odometry/Local_Map_Size/", (float)odom.data().id(), (float)odom.info().localMapSize); + _ui->statsToolBox->updateStat("Odometry/LocalMapSize/", (float)odom.data().id(), (float)odom.info().localMapSize); } _ui->statsToolBox->updateStat("Odometry/ID/", (float)odom.data().id(), (float)odom.data().id()); @@ -1158,7 +1196,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom) _ui->statsToolBox->updateStat("Odometry/Distance/m", (float)odom.data().id(), odom.info().distanceTravelled); } - _ui->statsToolBox->updateStat("/Gui Refresh Odom/ms", (float)odom.data().id(), time.elapsed()*1000.0); + _ui->statsToolBox->updateStat("GUI/Refresh odom/ms", (float)odom.data().id(), time.elapsed()*1000.0); _processingOdometry = false; } @@ -1433,6 +1471,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) std::map poses = stat.poses(); Transform groundTruthOffset = alignPosesToGroundTruth(poses, groundTruth); + UDEBUG("time= %d ms", time.restart()); updateMapCloud( poses, @@ -1447,7 +1486,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) _odometryCorrection = groundTruthOffset * stat.mapCorrection(); UDEBUG("time= %d ms", time.restart()); - _ui->statsToolBox->updateStat("/Gui RGB-D cloud/ms", stat.refImageId(), int(timerVis.elapsed()*1000.0f)); + _ui->statsToolBox->updateStat("GUI/RGB-D cloud/ms", stat.refImageId(), int(timerVis.elapsed()*1000.0f)); // loop closure view if((stat.loopClosureId() > 0 || stat.localLoopClosureId() > 0) && @@ -1463,11 +1502,117 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) UTimer loopTimer; _ui->widget_loopClosureViewer->updateView(); UINFO("Updating loop closure cloud view time=%fs", loopTimer.elapsed()); - _ui->statsToolBox->updateStat("/Gui RGB-D closure view/ms", stat.refImageId(), int(loopTimer.elapsed()*1000.0f)); + _ui->statsToolBox->updateStat("GUI/RGB-D closure view/ms", stat.refImageId(), int(loopTimer.elapsed()*1000.0f)); } UDEBUG("time= %d ms", time.restart()); } + + // ground truth live statistics + if(poses.size() && groundTruth.size()) + { + std::vector translationalErrors(poses.size()); + std::vector 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::iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + std::map::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/", stat.refImageId(), translational_rmse); + _ui->statsToolBox->updateStat("GT/translational_mean/", stat.refImageId(), translational_mean); + _ui->statsToolBox->updateStat("GT/translational_median/", stat.refImageId(), translational_median); + _ui->statsToolBox->updateStat("GT/translational_std/", stat.refImageId(), translational_std); + _ui->statsToolBox->updateStat("GT/translational_min/", stat.refImageId(), translational_min); + _ui->statsToolBox->updateStat("GT/translational_max/", stat.refImageId(), translational_max); + + _ui->statsToolBox->updateStat("GT/rotational_rmse/", stat.refImageId(), rotational_rmse); + _ui->statsToolBox->updateStat("GT/rotational_mean/", stat.refImageId(), rotational_mean); + _ui->statsToolBox->updateStat("GT/rotational_median/", stat.refImageId(), rotational_median); + _ui->statsToolBox->updateStat("GT/rotational_std/", stat.refImageId(), rotational_std); + _ui->statsToolBox->updateStat("GT/rotational_min/", stat.refImageId(), rotational_min); + _ui->statsToolBox->updateStat("GT/rotational_max/", stat.refImageId(), rotational_max); + } + } + UDEBUG("time= %d ms", time.restart()); } if( _ui->graphicsView_graphView->isVisible()) @@ -1505,7 +1650,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) } float elapsedTime = static_cast(totalTime.elapsed()); UINFO("Updating GUI time = %fs", elapsedTime/1000.0f); - _ui->statsToolBox->updateStat("/Gui Refresh Stats/ms", stat.refImageId(), elapsedTime); + _ui->statsToolBox->updateStat("GUI/Refresh stats/ms", stat.refImageId(), elapsedTime); if(_ui->actionAuto_screen_capture->isChecked() && !_autoScreenCaptureOdomSync) { this->captureScreen(_autoScreenCaptureRAM); @@ -2299,7 +2444,6 @@ Transform MainWindow::alignPosesToGroundTruth( std::map & poses, const std::map & groundTruth) { - UDEBUG(""); Transform t = Transform::getIdentity(); if(groundTruth.size() && poses.size()) { @@ -2322,14 +2466,15 @@ Transform MainWindow::alignPosesToGroundTruth( cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z()); } } - if(oi>1) + + if(oi>5) { cloud1.resize(oi); cloud2.resize(oi); t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1); } - else if(oi==1) + else if(idFirst) { t = groundTruth.at(idFirst) * poses.at(idFirst).inverse(); } @@ -3272,14 +3417,6 @@ void MainWindow::startDetection() } } - if(!_preferencesDialog->isCloudsShown(0) || !_preferencesDialog->isScansShown(0)) - { - QMessageBox::information(this, - tr("Some data may not be shown!"), - tr("Note that clouds and/or scans visibility settings are set to " - "OFF (see General->\"3D Rendering\" section under Map column).")); - } - UDEBUG(""); emit stateChanged(kStartingDetection); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 4a0bcc99..a7259032 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -667,6 +667,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->localDetection_radius->setObjectName(Parameters::kRGBDLocalRadius().c_str()); _ui->localDetection_maxDiffID->setObjectName(Parameters::kRGBDProximityMaxGraphDepth().c_str()); _ui->localDetection_pathFilteringRadius->setObjectName(Parameters::kRGBDProximityPathFilteringRadius().c_str()); + _ui->localDetection_angle->setObjectName(Parameters::kRGBDProximityAngle().c_str()); _ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDProximityPathRawPosesUsed().c_str()); _ui->checkBox_localSpaceAssembleScans->setObjectName(Parameters::kRGBDProximityPathScansMerged().c_str()); _ui->rgdb_localImmunizationRatio->setObjectName(Parameters::kRGBDLocalImmunizationRatio().c_str()); @@ -1160,7 +1161,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->doubleSpinBox_mesh_angleTolerance->setValue(15.0); #if PCL_VERSION_COMPARE(>=, 1, 7, 2) - _ui->groupBox_organized->setChecked(true); + _ui->groupBox_organized->setChecked(false); _ui->checkBox_mesh_quad->setChecked(true); #else _ui->groupBox_organized->setChecked(false); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index e80a972e..a6248a65 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,7 +63,7 @@ 0 - -249 + 0 681 2010 @@ -86,7 +86,7 @@ QFrame::Raised - 14 + 11 @@ -6615,7 +6615,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + m @@ -6631,7 +6631,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Path filtering radius to avoid merging laser scans which are close. 0 to ignore. @@ -6644,7 +6644,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + When comparing to a local path, merge the laser scans using the odometry poses instead of the ones in the optimized local graph. @@ -6657,7 +6657,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + @@ -6687,7 +6687,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Save scan matching IDs in link's user data. @@ -6700,14 +6700,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - + Merge close laser scans on each path. If false, only the nearest laser scan on the path is used for ICP. @@ -6720,13 +6720,42 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + + + + + Maximum angle for visual proximity detection. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + degrees + + + 0 + + + 360.000000000000000 + + + 10.000000000000000 + + +