mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-03 16:27:46 +08:00
Added parameter "RGBD/ProximityAngle"
Updated default of GuessMotion parameters Proximity detection by time: added guess transform based on optimized poses RegistrationVis: ByPass OpticalFlow if no images are detected (use features matching instead if features are set) solvePnPRansac() refining: using std dev instead of variance Statistics panel: creating Camera/GUI/Odometry panels without the need to start detection (issue #51) Statistics: Adding a GT panel when ground truth poses are detected with RMSE info
This commit is contained in:
matlabbe
@@ -181,7 +181,7 @@ public:
|
||||
std::multimap<int, Link> & 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,
|
||||
|
||||
@@ -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.");
|
||||
|
||||
@@ -195,6 +195,7 @@ private:
|
||||
float _proximityFilteringRadius;
|
||||
bool _proximityRawPosesUsed;
|
||||
bool _proximityScansMerged;
|
||||
float _proximityAngle;
|
||||
std::string _databasePath;
|
||||
bool _optimizeFromGraphEnd;
|
||||
float _optimizationMaxLinearError;
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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::KeyPoint>(), 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);
|
||||
|
||||
@@ -288,7 +288,9 @@ Transform RegistrationVis::computeTransformationImpl(
|
||||
std::multimap<int, cv::Point3f> words3To;
|
||||
std::multimap<int, cv::Mat> wordsDescFrom;
|
||||
std::multimap<int, cv::Mat> 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;
|
||||
|
||||
+13
-4
@@ -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)
|
||||
|
||||
@@ -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<int> 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);
|
||||
|
||||
+161
-24
@@ -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<int, Transform> 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<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/", 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<float>(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<int, Transform> & poses,
|
||||
const std::map<int, Transform> & 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);
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -63,7 +63,7 @@
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>-249</y>
|
||||
<y>0</y>
|
||||
<width>681</width>
|
||||
<height>2010</height>
|
||||
</rect>
|
||||
@@ -86,7 +86,7 @@
|
||||
<enum>QFrame::Raised</enum>
|
||||
</property>
|
||||
<property name="currentIndex">
|
||||
<number>14</number>
|
||||
<number>11</number>
|
||||
</property>
|
||||
<widget class="QWidget" name="page_22">
|
||||
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
|
||||
@@ -6615,7 +6615,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
|
||||
</item>
|
||||
<item>
|
||||
<layout class="QGridLayout" name="gridLayout_50" columnstretch="0,1">
|
||||
<item row="2" column="0">
|
||||
<item row="3" column="0">
|
||||
<widget class="QDoubleSpinBox" name="localDetection_pathFilteringRadius">
|
||||
<property name="suffix">
|
||||
<string> m</string>
|
||||
@@ -6631,7 +6631,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="2" column="1">
|
||||
<item row="3" column="1">
|
||||
<widget class="QLabel" name="label_space3_3">
|
||||
<property name="text">
|
||||
<string>Path filtering radius to avoid merging laser scans which are close. 0 to ignore.</string>
|
||||
@@ -6644,7 +6644,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="3" column="1">
|
||||
<item row="4" column="1">
|
||||
<widget class="QLabel" name="label_scanMatching_4">
|
||||
<property name="text">
|
||||
<string>When comparing to a local path, merge the laser scans using the odometry poses instead of the ones in the optimized local graph.</string>
|
||||
@@ -6657,7 +6657,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="3" column="0">
|
||||
<item row="4" column="0">
|
||||
<widget class="QCheckBox" name="checkBox_localSpacePathOdomPosesUsed">
|
||||
<property name="text">
|
||||
<string/>
|
||||
@@ -6687,7 +6687,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="5" column="1">
|
||||
<item row="6" column="1">
|
||||
<widget class="QLabel" name="label_scanMatching_6">
|
||||
<property name="text">
|
||||
<string>Save scan matching IDs in link's user data.</string>
|
||||
@@ -6700,14 +6700,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="5" column="0">
|
||||
<item row="6" column="0">
|
||||
<widget class="QCheckBox" name="checkBox_localSpaceScanMatchingIDsSaved">
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<item row="2" column="1">
|
||||
<widget class="QLabel" name="label_scanMatching_7">
|
||||
<property name="text">
|
||||
<string>Merge close laser scans on each path. If false, only the nearest laser scan on the path is used for ICP.</string>
|
||||
@@ -6720,13 +6720,42 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<item row="2" column="0">
|
||||
<widget class="QCheckBox" name="checkBox_localSpaceAssembleScans">
|
||||
<property name="text">
|
||||
<string/>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
<widget class="QLabel" name="label_scanMatching_8">
|
||||
<property name="text">
|
||||
<string>Maximum angle for visual proximity detection.</string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
</property>
|
||||
<property name="textInteractionFlags">
|
||||
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="0">
|
||||
<widget class="QDoubleSpinBox" name="localDetection_angle">
|
||||
<property name="suffix">
|
||||
<string> degrees</string>
|
||||
</property>
|
||||
<property name="decimals">
|
||||
<number>0</number>
|
||||
</property>
|
||||
<property name="maximum">
|
||||
<double>360.000000000000000</double>
|
||||
</property>
|
||||
<property name="singleStep">
|
||||
<double>10.000000000000000</double>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
</item>
|
||||
</layout>
|
||||
|
||||
Reference in New Issue
Block a user