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

Pnp multicam refactoring (#902)

Browse Source 
* gui: fixed wrongly showing landmark rejected when it was not (because a loop closure was rejected at the same time)

* Added Vis/PnPMaxVariance and RGBD/InvertedReg parameters. Implemented inlier distribution computation for multicam.

* On loc/small displacement: don't remove from odom cache if loop is rejected (maybe first loc)

* Loc: don't prune odom cache on small movement if delayed loc is enabled

* loc/small movement: cleanup bidirectional links

* Cov/PnP: fixed objPt transform to estimate depth

Co-authored-by: mathieu86 <mathieu@robust.ai>
This commit is contained in:
matlabbe
2022-09-24 12:29:42 -07:00
committed by GitHub
co-authored by mathieu86
parent 95a76cb696
commit fa31affea0
14 changed files with 685 additions and 495 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
#######################
SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 20)
SET(RTABMAP_PATCH_VERSION 20)
SET(RTABMAP_PATCH_VERSION 21)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
corelib/include/rtabmap/core/Memory.h
+1
View File
@@ -324,6 +324,7 @@ private:
float _laserScanGroundNormalsUp;
bool _reextractLoopClosureFeatures;
bool _localBundleOnLoopClosure;
bool _invertedReg;
float _rehearsalMaxDistance;
float _rehearsalMaxAngle;
bool _rehearsalWeightIgnoredWhileMoving;
corelib/include/rtabmap/core/Parameters.h
+2
View File
@@ -370,6 +370,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, LoopClosureIdentityGuess, bool, false, uFormat("Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used, thus assuming that registration strategy selected (%s) can deal with transformation estimation without guess.", kRegStrategy().c_str()));
RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, false, "Extract features even if there are some already in the nodes. Raw features are not saved in database.");
RTABMAP_PARAM(RGBD, LocalBundleOnLoopClosure, bool, false, "Do local bundle adjustment with neighborhood of the loop closure.");
RTABMAP_PARAM(RGBD, InvertedReg, bool, false, "On loop closure, do registration from the target to reference instead of reference to target.");
RTABMAP_PARAM(RGBD, CreateOccupancyGrid, bool, false, "Create local occupancy grid maps. See \"Grid\" group for parameters.");
RTABMAP_PARAM(RGBD, MarkerDetection, bool, false, "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Marker\" group for parameters.");
RTABMAP_PARAM(RGBD, LoopCovLimited, bool, false, "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
@@ -594,6 +595,7 @@ class RTABMAP_EXP Parameters
#else
RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
#endif
RTABMAP_PARAM(Vis, PnPMaxVariance, float, 0.0, uFormat("[%s = 1] Max linear variance between 3D point correspondences after PnP. 0 means disabled.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.1, uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, MinInliers, int, 20, "Minimum feature correspondences to compute/accept the transformation.");
corelib/include/rtabmap/core/RegistrationVis.h
+1
View File
@@ -82,6 +82,7 @@ private:
float _PnPReprojError;
int _PnPFlags;
int _PnPRefineIterations;
float _PnPMaxVar;
int _correspondencesApproach;
int _flowWinSize;
int _flowIterations;
corelib/include/rtabmap/core/util3d_motion_estimation.h
+2
View File
@@ -48,6 +48,7 @@ Transform RTABMAP_EXP estimateMotion3DTo2D(
double reprojError = 5.,
int flagsPnP = 0,
int pnpRefineIterations = 1,
float maxVariance = 0,
const Transform & guess = Transform::getIdentity(),
const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
cv::Mat * covariance = 0, // mean reproj error if words3B is not set
@@ -63,6 +64,7 @@ Transform RTABMAP_EXP estimateMotion3DTo2D(
double reprojError = 5.,
int flagsPnP = 0,
int pnpRefineIterations = 1,
float maxVariance = 0,
const Transform & guess = Transform::getIdentity(),
const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
cv::Mat * covariance = 0, // mean reproj error if words3B is not set
corelib/src/Memory.cpp
+30 -2
View File
@@ -101,6 +101,7 @@ Memory::Memory(const ParametersMap & parameters) :
_laserScanGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
_localBundleOnLoopClosure(Parameters::defaultRGBDLocalBundleOnLoopClosure()),
_invertedReg(Parameters::defaultRGBDInvertedReg()),
_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
@@ -578,6 +579,15 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kIcpPointToPlaneGroundNormalsUp(), _laserScanGroundNormalsUp);
Parameters::parse(params, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
Parameters::parse(params, Parameters::kRGBDLocalBundleOnLoopClosure(), _localBundleOnLoopClosure);
Parameters::parse(params, Parameters::kRGBDInvertedReg(), _invertedReg);
if(_invertedReg && _localBundleOnLoopClosure)
{
UWARN("%s and %s cannot be used at the same time, disabling %s...",
Parameters::kRGBDLocalBundleOnLoopClosure().c_str(),
Parameters::kRGBDInvertedReg().c_str(),
Parameters::kRGBDLocalBundleOnLoopClosure().c_str());
_localBundleOnLoopClosure = false;
}
Parameters::parse(params, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
Parameters::parse(params, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
@@ -2855,8 +2865,21 @@ Transform Memory::computeTransform(
(fromS.getWords().size() && toS.getWords().size()) ||
(!guess.isNull() && !_registrationPipeline->isImageRequired()))
{
Signature tmpFrom = fromS;
Signature tmpTo = toS;
Signature tmpFrom, tmpTo;
if(_invertedReg)
{
tmpFrom = toS;
tmpTo = fromS;
if(!guess.isNull())
{
guess = guess.inverse();
}
}
else
{
tmpFrom = fromS;
tmpTo = toS;
}
if(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))
{
@@ -2891,6 +2914,7 @@ Transform Memory::computeTransform(
_registrationPipeline->isImageRequired() &&
!_registrationPipeline->isScanRequired() &&
!_registrationPipeline->isUserDataRequired() &&
!_invertedReg &&
!tmpTo.getWordsDescriptors().empty() &&
!tmpTo.getWords().empty() &&
!tmpFrom.getWordsDescriptors().empty() &&
@@ -3115,6 +3139,10 @@ Transform Memory::computeTransform(
{
transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
}
if(_invertedReg && !transform.isNull())
{
transform = transform.inverse();
}
}
return transform;
}
corelib/src/RegistrationVis.cpp
+27 -20
View File
@@ -69,6 +69,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_PnPReprojError(Parameters::defaultVisPnPReprojError()),
_PnPFlags(Parameters::defaultVisPnPFlags()),
_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
_PnPMaxVar(Parameters::defaultVisPnPMaxVariance()),
_correspondencesApproach(Parameters::defaultVisCorType()),
_flowWinSize(Parameters::defaultVisCorFlowWinSize()),
_flowIterations(Parameters::defaultVisCorFlowIterations()),
@@ -124,6 +125,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations);
Parameters::parse(parameters, Parameters::kVisPnPMaxVariance(), _PnPMaxVar);
Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
@@ -287,6 +289,7 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
UDEBUG("%s=%f", Parameters::kVisPnPMaxVariance().c_str(), _PnPMaxVar);
UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
@@ -1580,6 +1583,7 @@ Transform RegistrationVis::computeTransformationImpl(
_PnPReprojError,
_PnPFlags,
_PnPRefineIterations,
_PnPMaxVar,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
words3B,
&covariances[dir],
@@ -1601,6 +1605,7 @@ Transform RegistrationVis::computeTransformationImpl(
_PnPReprojError,
_PnPFlags,
_PnPRefineIterations,
_PnPMaxVar,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
words3B,
&covariances[dir],
@@ -1977,31 +1982,31 @@ Transform RegistrationVis::computeTransformationImpl(
if(!transform.isNull() && !allInliers.empty() && (_minInliersDistributionThr>0.0f || _maxInliersMeanDistance>0.0f))
{
cv::Mat pcaData;
float cx=0, cy=0, w=0, h=0;
std::vector<CameraModel> cameraModelsTo;
if(toSignature.sensorData().stereoCameraModels().size())
{
for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
{
cameraModelsTo.push_back(toSignature.sensorData().stereoCameraModels()[i].left());
}
}
else
{
cameraModelsTo = toSignature.sensorData().cameraModels();
}
if(_minInliersDistributionThr > 0)
{
if((toSignature.sensorData().stereoCameraModels().size() == 1 && toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) ||
(toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForReprojection()))
if(cameraModelsTo.size() >= 1 && cameraModelsTo[0].isValidForReprojection())
{
const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModels().size()?toSignature.sensorData().stereoCameraModels()[0].left():toSignature.sensorData().cameraModels()[0];
cx = cameraModel.cx();
cy = cameraModel.cy();
w = cameraModel.imageWidth();
h = cameraModel.imageHeight();
if(w>0 && h>0)
if(cameraModelsTo[0].imageWidth()>0 && cameraModelsTo[0].imageHeight()>0)
{
pcaData = cv::Mat(allInliers.size(), 2, CV_32FC1);
}
else
{
UERROR("Invalid calibration image size (%dx%d), cannot compute inliers distribution! (see %s=%f)", w, h, Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
UERROR("Invalid calibration image size (%dx%d), cannot compute inliers distribution! (see %s=%f)", cameraModelsTo[0].imageWidth(), cameraModelsTo[0].imageHeight(), Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
}
}
else if(toSignature.sensorData().cameraModels().size() > 1 || toSignature.sensorData().stereoCameraModels().size() > 1)
{
UERROR("Multi-camera not supported when computing inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
}
else
{
UERROR("Calibration not valid, cannot compute inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
@@ -2031,12 +2036,14 @@ Transform RegistrationVis::computeTransformationImpl(
if(!pcaData.empty())
{
std::multimap<int, int>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
UASSERT(wordsIter != fromSignature.getWords().end() && !fromSignature.getWordsKpts().empty());
std::multimap<int, int>::const_iterator wordsIter = toSignature.getWords().find(allInliers[i]);
UASSERT(wordsIter != fromSignature.getWords().end() && !toSignature.getWordsKpts().empty());
float * ptr = pcaData.ptr<float>(i, 0);
const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[wordsIter->second];
ptr[0] = (kpt.pt.x-cx) / w;
ptr[1] = (kpt.pt.y-cy) / h;
const cv::KeyPoint & kpt = toSignature.getWordsKpts()[wordsIter->second];
int cameraIndex = (int)(kpt.pt.x / cameraModelsTo[0].imageWidth());
UASSERT_MSG(cameraIndex < (int)cameraModelsTo.size(), uFormat("cameraIndex=%d (x=%f models=%d camera width = %d)", cameraIndex, kpt.pt.x, (int)cameraModelsTo.size(), cameraModelsTo[0].imageWidth()).c_str());
ptr[0] = (kpt.pt.x-cameraIndex*cameraModelsTo[cameraIndex].imageWidth()-cameraModelsTo[cameraIndex].cx()) / cameraModelsTo[cameraIndex].imageWidth();
ptr[1] = (kpt.pt.y-cameraModelsTo[cameraIndex].cy()) / cameraModelsTo[cameraIndex].imageHeight();
}
}
corelib/src/Rtabmap.cpp
+47 -21
View File
@@ -1434,32 +1434,45 @@ bool Rtabmap::process(
//============================================================
// Minimum displacement required to add to Memory
//============================================================
const std::multimap<int, Link> & links = signature->getLinks();
if(links.size() && links.begin()->second.type() == Link::kNeighbor)
Transform t;
if(_memory->isIncremental())
{
const Signature * s = _memory->getSignature(links.begin()->second.to());
UASSERT(s!=0);
// don't filter if the new node is not intermediate but previous one is
if(signature->getWeight() < 0 || s->getWeight() >= 0)
const std::multimap<int, Link> & links = signature->getLinks();
if(links.size() && links.begin()->second.type() == Link::kNeighbor)
{
float x,y,z, roll,pitch,yaw;
links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
bool isMoving = fabs(x) > _rgbdLinearUpdate ||
fabs(y) > _rgbdLinearUpdate ||
fabs(z) > _rgbdLinearUpdate ||
(_rgbdAngularUpdate>0.0f && (
fabs(roll) > _rgbdAngularUpdate ||
fabs(pitch) > _rgbdAngularUpdate ||
fabs(yaw) > _rgbdAngularUpdate));
if(!isMoving)
const Signature * s = _memory->getSignature(links.begin()->second.to());
UASSERT(s!=0);
// don't filter if the new node is not intermediate but previous one is
if(signature->getWeight() < 0 || s->getWeight() >= 0)
{
// This will disable global loop closure detection, only retrieval will be done.
// The location will also be deleted at the end.
smallDisplacement = true;
UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
t = links.begin()->second.transform();
}
}
}
else if(!_odomCachePoses.empty())
{
t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose();
}
if(!t.isNull())
{
float x,y,z, roll,pitch,yaw;
t.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
bool isMoving = fabs(x) > _rgbdLinearUpdate ||
fabs(y) > _rgbdLinearUpdate ||
fabs(z) > _rgbdLinearUpdate ||
(_rgbdAngularUpdate>0.0f && (
fabs(roll) > _rgbdAngularUpdate ||
fabs(pitch) > _rgbdAngularUpdate ||
fabs(yaw) > _rgbdAngularUpdate));
if(!isMoving)
{
// This will disable global loop closure detection, only retrieval will be done.
// The location will also be deleted at the end.
smallDisplacement = true;
UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
}
}
}
if(odomVelocity.size() == 6)
{
@@ -2951,6 +2964,7 @@ bool Rtabmap::process(
cv::Mat localizationCovariance;
Transform previousMapCorrection;
bool rejectedLandmark = false;
bool delayedLocalization = false;
UDEBUG("RGB-D SLAM mode: %d", _rgbdSlamMode?1:0);
UDEBUG("Incremental: %d", _memory->isIncremental());
UDEBUG("Loop hyp: %d", _loopClosureHypothesis.first);
@@ -3441,6 +3455,7 @@ bool Rtabmap::process(
else //delayed localization (wait for more than 1 link)
{
UWARN("Localization was good, but waiting for another one to be more accurate (%s>0)", Parameters::kRGBDMaxOdomCacheSize().c_str());
delayedLocalization = true;
rejectLocalization = true;
}
}
@@ -3954,10 +3969,21 @@ bool Rtabmap::process(
(smallDisplacement || tooFastMovement) &&
_loopClosureHypothesis.first == 0 &&
lastProximitySpaceClosureId == 0 &&
!delayedLocalization &&
(rejectedLandmark || landmarksDetected.empty()))
{
_odomCachePoses.erase(signatureRemoved);
_odomCacheConstraints.erase(signatureRemoved);
for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)
{
if(iter->second.from() == signatureRemoved || iter->second.to() == signatureRemoved)
{
_odomCacheConstraints.erase(iter++);
}
else
{
++iter;
}
}
}
// Pass this point signature should not be used, since it could have been transferred...
corelib/src/util3d_motion_estimation.cpp
+92 -110
View File
@@ -61,6 +61,7 @@ Transform estimateMotion3DTo2D(
double reprojError,
int flagsPnP,
int refineIterations,
float maxVariance,
const Transform & guess,
const std::map<int, cv::Point3f> & words3B,
cv::Mat * covariance,
@@ -150,69 +151,59 @@ Transform estimateMotion3DTo2D(
{
std::vector<float> errorSqrdDists(inliers.size());
std::vector<float> errorSqrdAngles(inliers.size());
oi = 0;
Transform transformCameraFrame = transform * cameraModel.localTransform();
Transform transformCameraFrameInv = transformCameraFrame.inverse();
Transform localTransformInv = cameraModel.localTransform().inverse();
Transform transformCameraFrameInv = (transform * cameraModel.localTransform()).inverse();
for(unsigned int i=0; i<inliers.size(); ++i)
{
cv::Point3f objPt = objectPoints[inliers[i]];
// Project obj point from base frame of cameraA in cameraB frame (z+ in front of the cameraB)
objPt = util3d::transformPoint(objPt, transformCameraFrameInv);
// Get 3D point from target in cameraB frame
std::map<int, cv::Point3f>::const_iterator iter = words3B.find(matches[inliers[i]]);
if(words3B.empty() || (iter != words3B.end() && util3d::isFinite(iter->second)))
cv::Point3f newPt;
if(iter!=words3B.end() && util3d::isFinite(iter->second))
{
const cv::Point3f & objPt = objectPoints[inliers[i]];
cv::Point3f newPt;
if(iter!=words3B.end())
{
newPt = util3d::transformPoint(iter->second, transform);
}
else
{
//compute from projection
Eigen::Vector3f ray = projectDepthTo3DRay(
cameraModel.imageSize(),
imagePoints.at(inliers[i]).x,
imagePoints.at(inliers[i]).y,
cameraModel.cx(),
cameraModel.cy(),
cameraModel.fx(),
cameraModel.fy());
// transform in camera B frame
newPt = util3d::transformPoint(objPt, transformCameraFrameInv);
newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * newPt.z*1.1; // Add 10 % error
// put back in frame of camera A
newPt = util3d::transformPoint(newPt, transformCameraFrame);
}
errorSqrdDists[oi] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
Eigen::Vector4f v1(objPt.x - transform.x(), objPt.y - transform.y(), objPt.z - transform.z(), 0);
Eigen::Vector4f v2(newPt.x - transform.x(), newPt.y - transform.y(), newPt.z - transform.z(), 0);
errorSqrdAngles[oi++] = pcl::getAngle3D(v1, v2);
newPt = util3d::transformPoint(iter->second, localTransformInv);
}
else
{
//compute from projection
Eigen::Vector3f ray = projectDepthTo3DRay(
cameraModel.imageSize(),
imagePoints.at(inliers[i]).x,
imagePoints.at(inliers[i]).y,
cameraModel.cx(),
cameraModel.cy(),
cameraModel.fx(),
cameraModel.fy());
// transform in camera B frame
newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPt.z*1.1; // Add 10 % error
}
errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
Eigen::Vector4f v1(objPt.x, objPt.y, objPt.z, 0);
Eigen::Vector4f v2(newPt.x, newPt.y, newPt.z, 0);
errorSqrdAngles[i] = pcl::getAngle3D(v1, v2);
}
errorSqrdDists.resize(oi);
errorSqrdAngles.resize(oi);
if(errorSqrdDists.size())
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
//divide by 4 instead of 2 to ignore very very far features (stereo)
double median_error_sqr = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2];
UASSERT(uIsFinite(median_error_sqr));
(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr;
std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
median_error_sqr = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2];
UASSERT(uIsFinite(median_error_sqr));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr;
}
else
{
UWARN("Not enough close points to compute covariance!");
}
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
//divide by 4 instead of 2 to ignore very very far features (stereo)
double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2];
UASSERT(uIsFinite(median_error_sqr_lin));
(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin;
std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2];
UASSERT(uIsFinite(median_error_sqr_ang));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;
if(float(oi) / float(inliers.size()) < 0.2f)
if(maxVariance > 0 && median_error_sqr_lin > maxVariance)
{
UWARN("A very low number of inliers have valid depth (%d/%d), the transform returned may be wrong!", oi, (int)inliers.size());
UWARN("Rejected PnP transform, variance is too high! %f > %f!", median_error_sqr_lin, maxVariance);
*covariance = cv::Mat::eye(6,6,CV_64FC1);
transform.setNull();
}
}
else if(covariance)
@@ -256,6 +247,7 @@ Transform estimateMotion3DTo2D(
double reprojError,
int flagsPnP,
int refineIterations,
float maxVariance,
const Transform & guess,
const std::map<int, cv::Point3f> & words3B,
cv::Mat * covariance,
@@ -273,7 +265,7 @@ Transform estimateMotion3DTo2D(
UASSERT(cameraModels[i].isValidForProjection());
UASSERT(subImageWidth == cameraModels[i].imageWidth());
}
UASSERT(!guess.isNull());
std::vector<int> matches, inliers;
@@ -391,70 +383,60 @@ Transform estimateMotion3DTo2D(
{
std::vector<float> errorSqrdDists(inliers.size());
std::vector<float> errorSqrdAngles(inliers.size());
oi = 0;
for(unsigned int i=0; i<inliers.size(); ++i)
{
cv::Point3f objPt = objectPoints[inliers[i]];
int cameraIndex = cameraIndexes[inliers[i]];
Transform transformCameraFrameInv = (transform * cameraModels[cameraIndex].localTransform()).inverse();
// Project obj point from base frame of cameraA in cameraB frame (z+ in front of the cameraB)
objPt = util3d::transformPoint(objPt, transformCameraFrameInv);
// Get 3D point from target in cameraB frame
std::map<int, cv::Point3f>::const_iterator iter = words3B.find(matches[inliers[i]]);
if(words3B.empty() || (iter != words3B.end() && util3d::isFinite(iter->second)))
cv::Point3f newPt;
if(iter!=words3B.end() && util3d::isFinite(iter->second))
{
const cv::Point3f & objPt = objectPoints[inliers[i]];
cv::Point3f newPt;
if(iter!=words3B.end())
{
newPt = util3d::transformPoint(iter->second, transform);
}
else
{
//compute from projection
int cameraIndex = cameraIndexes[inliers[i]];
Transform transformCameraFrame = transform * cameraModels[cameraIndex].localTransform();
Transform transformCameraFrameInv = transformCameraFrame.inverse();
Eigen::Vector3f ray = projectDepthTo3DRay(
cameraModels[cameraIndex].imageSize(),
imagePoints.at(inliers[i]).x,
imagePoints.at(inliers[i]).y,
cameraModels[cameraIndex].cx(),
cameraModels[cameraIndex].cy(),
cameraModels[cameraIndex].fx(),
cameraModels[cameraIndex].fy());
// transform in camera B frame
newPt = util3d::transformPoint(objPt, transformCameraFrameInv);
newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * newPt.z*1.1; // Add 10 % error
// put back in frame of camera A
newPt = util3d::transformPoint(newPt, transformCameraFrame);
}
errorSqrdDists[oi] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
Eigen::Vector4f v1(objPt.x - transform.x(), objPt.y - transform.y(), objPt.z - transform.z(), 0);
Eigen::Vector4f v2(newPt.x - transform.x(), newPt.y - transform.y(), newPt.z - transform.z(), 0);
errorSqrdAngles[oi++] = pcl::getAngle3D(v1, v2);
newPt = util3d::transformPoint(iter->second, cameraModels[cameraIndex].localTransform().inverse());
}
else
{
//compute from projection
Eigen::Vector3f ray = projectDepthTo3DRay(
cameraModels[cameraIndex].imageSize(),
imagePoints.at(inliers[i]).x,
imagePoints.at(inliers[i]).y,
cameraModels[cameraIndex].cx(),
cameraModels[cameraIndex].cy(),
cameraModels[cameraIndex].fx(),
cameraModels[cameraIndex].fy());
// transform in camera B frame
newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPt.z*1.1; // Add 10 % error
}
errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
Eigen::Vector4f v1(objPt.x, objPt.y, objPt.z, 0);
Eigen::Vector4f v2(newPt.x, newPt.y, newPt.z, 0);
errorSqrdAngles[i] = pcl::getAngle3D(v1, v2);
}
errorSqrdDists.resize(oi);
errorSqrdAngles.resize(oi);
if(errorSqrdDists.size())
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
//divide by 4 instead of 2 to ignore very very far features (stereo)
double median_error_sqr = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2];
UASSERT(uIsFinite(median_error_sqr));
(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr;
std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
median_error_sqr = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2];
UASSERT(uIsFinite(median_error_sqr));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr;
}
else
{
UWARN("Not enough close points to compute covariance!");
}
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
//divide by 4 instead of 2 to ignore very very far features (stereo)
double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2];
UASSERT(uIsFinite(median_error_sqr_lin));
(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin;
std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2];
UASSERT(uIsFinite(median_error_sqr_ang));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;
if(float(oi) / float(inliers.size()) < 0.2f)
if(maxVariance > 0 && median_error_sqr_lin > maxVariance)
{
UWARN("A very low number of inliers have valid depth (%d/%d), the transform returned may be wrong!", oi, (int)inliers.size());
UWARN("Rejected PnP transform, variance is too high! %f > %f!", median_error_sqr_lin, maxVariance);
*covariance = cv::Mat::eye(6,6,CV_64FC1);
transform.setNull();
}
}
}
guilib/include/rtabmap/gui/ImageView.h
+4
View File
@@ -63,6 +63,7 @@ public:
bool isLinesShown() const;
int getAlpha() const {return _alpha;}
int getFeaturesSize() const {return _featuresSize;}
int getLinesWidth() const {return _linesWidth;}
bool isGraphicsViewMode() const;
bool isGraphicsViewScaled() const;
bool isGraphicsViewScaledToHeight() const;
@@ -101,6 +102,7 @@ public:
void setFeaturesColor(QColor color);
void setAlpha(int alpha);
void setFeaturesSize(int size);
void setLinesWidth(int width);
void setSceneRect(const QRectF & rect);
const QMultiMap<int, rtabmap::KeypointItem *> & getFeatures() const {return _features;}
@@ -131,6 +133,7 @@ private:
QString _savedFileName;
int _alpha;
int _featuresSize;
int _linesWidth;
QColor _defaultBgColor;
QColor _defaultFeatureColor;
QColor _defaultMatchingFeatureColor;
@@ -148,6 +151,7 @@ private:
QAction * _saveImage;
QAction * _setAlpha;
QAction * _setFeaturesSize;
QAction * _setLinesWidth;
QAction * _graphicsViewMode;
QAction * _graphicsViewScaled;
QAction * _graphicsViewScaledToHeight;
guilib/src/ImageView.cpp
+37 -3
View File
@@ -68,7 +68,11 @@ public:
delete _placeHolder;
}
void setColor(const QColor & color);
void setWidth(int width)
{
_width = width;
this->setPen(QPen(pen().color(), _width));
}
protected:
virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
@@ -166,6 +170,7 @@ ImageView::ImageView(QWidget * parent) :
QWidget(parent),
_alpha(100),
_featuresSize(0.0f),
_linesWidth(0),
_defaultBgColor(Qt::black),
_defaultFeatureColor(Qt::yellow),
_defaultMatchingFeatureColor(Qt::magenta),
@@ -211,6 +216,7 @@ ImageView::ImageView(QWidget * parent) :
_showLines = _featureMenu->addAction(tr("Show lines"));
_showLines->setCheckable(true);
_showLines->setChecked(true);
_setLinesWidth = _featureMenu->addAction(tr("Set lines width..."));
_setFeatureColor = _featureMenu->addAction(tr("Set default feature color"));
_setFeatureColor->setIcon(createIcon(_defaultFeatureColor));
_setFeatureColor->setIconVisibleInMenu(true);
@@ -280,6 +286,7 @@ void ImageView::saveSettings(QSettings & settings, const QString & group) const
settings.setValue("features_shown", this->isFeaturesShown());
settings.setValue("features_size", this->getFeaturesSize());
settings.setValue("lines_shown", this->isLinesShown());
settings.setValue("lines_width", this->getLinesWidth());
settings.setValue("alpha", this->getAlpha());
settings.setValue("bg_color", this->getDefaultBackgroundColor());
settings.setValue("feature_color", this->getDefaultFeatureColor());
@@ -307,6 +314,7 @@ void ImageView::loadSettings(QSettings & settings, const QString & group)
this->setFeaturesShown(settings.value("features_shown", this->isFeaturesShown()).toBool());
this->setFeaturesSize(settings.value("features_size", this->getFeaturesSize()).toInt());
this->setLinesShown(settings.value("lines_shown", this->isLinesShown()).toBool());
this->setLinesWidth(settings.value("lines_width", this->getLinesWidth()).toInt());
this->setAlpha(settings.value("alpha", this->getAlpha()).toInt());
this->setDefaultBackgroundColor(settings.value("bg_color", this->getDefaultBackgroundColor()).value<QColor>());
this->setDefaultFeatureColor(settings.value("feature_color", this->getDefaultFeatureColor()).value<QColor>());
@@ -796,9 +804,8 @@ void ImageView::paintEvent(QPaintEvent *event)
{
for(QList<QGraphicsLineItem*>::iterator iter = _lines.begin(); iter != _lines.end(); ++iter)
{
QColor color = (*iter)->pen().color();
painter.save();
painter.setPen(color);
painter.setPen(QPen((*iter)->pen().color(), _linesWidth));
painter.drawLine((*iter)->line());
painter.restore();
}
@@ -996,6 +1003,16 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
Q_EMIT configChanged();
}
}
else if(action == _setLinesWidth)
{
bool ok = false;
int value = QInputDialog::getInt(this, tr("Set lines width"), tr("Width"), _linesWidth, 0, 999, 1, &ok);
if(ok)
{
this->setLinesWidth(value);
Q_EMIT configChanged();
}
}
if(action == _showImage || action ==_showImageDepth)
{
@@ -1112,6 +1129,7 @@ void ImageView::addLine(float x1, float y1, float x2, float y2, QColor color, co
color.setAlpha(this->getAlpha());
LineItem * item = new LineItem(x1, y1, x2, y2, text);
item->setPen(QPen(color));
item->setWidth(_linesWidth);
_lines.push_back(item);
item->setVisible(isLinesShown());
item->setZValue(1);
@@ -1273,6 +1291,22 @@ void ImageView::setFeaturesSize(int size)
}
}
void ImageView::setLinesWidth(int width)
{
_linesWidth = width;
for(QList<QGraphicsLineItem*>::iterator iter=_lines.begin(); iter!=_lines.end(); ++iter)
{
if(dynamic_cast<LineItem*>(*iter))
{
((LineItem*)(*iter))->setWidth(_linesWidth);
}
}
if(!_graphicsView->isVisible())
{
this->update();
}
}
void ImageView::setSceneRect(const QRectF & rect)
{
_graphicsView->scene()->setSceneRect(rect);
guilib/src/MainWindow.cpp
+83 -37
View File
@@ -2186,12 +2186,30 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
//draw markers
if(!signature.getLandmarks().empty())
{
refImage = refImage.clone();
if(refImage.channels() == 1)
{
cv::Mat imgColor;
cvtColor(refImage, imgColor, cv::COLOR_GRAY2BGR);
refImage = imgColor;
}
else
{
refImage = refImage.clone();
}
drawLandmarks(refImage, signature);
}
if(!loopSignature.getLandmarks().empty())
{
loopImage = loopImage.clone();
if(loopImage.channels() == 1)
{
cv::Mat imgColor;
cv::cvtColor(loopImage, imgColor, cv::COLOR_GRAY2BGR);
loopImage = imgColor;
}
else
{
loopImage = loopImage.clone();
}
drawLandmarks(loopImage, loopSignature);
}
}
@@ -4910,44 +4928,72 @@ void MainWindow::drawLandmarks(cv::Mat & image, const Signature & signature)
{
for(std::map<int, Link>::const_iterator iter=signature.getLandmarks().begin(); iter!=signature.getLandmarks().end(); ++iter)
{
CameraModel model;
if(!signature.sensorData().cameraModels().empty() &&
signature.sensorData().cameraModels()[0].isValidForProjection())
// Project in all cameras in which the landmark is visible
for(size_t i=0; i<signature.sensorData().cameraModels().size() || i<signature.sensorData().stereoCameraModels().size(); ++i)
{
model = signature.sensorData().cameraModels()[0];
}
else if(!signature.sensorData().stereoCameraModels().empty() &&
signature.sensorData().stereoCameraModels()[0].isValidForProjection())
{
model = signature.sensorData().stereoCameraModels()[0].left();
}
if(model.isValidForProjection())
{
Transform t = model.localTransform().inverse() * iter->second.transform();
cv::Vec3d rvec, tvec;
tvec.val[0] = t.x();
tvec.val[1] = t.y();
tvec.val[2] = t.z();
cv::Mat R;
t.rotationMatrix().convertTo(R, CV_64F);
cv::Rodrigues(R, rvec);
CameraModel model;
if(i<signature.sensorData().cameraModels().size())
{
model = signature.sensorData().cameraModels()[i];
}
else if(i<signature.sensorData().stereoCameraModels().size())
{
model = signature.sensorData().stereoCameraModels()[i].left();
}
if(model.isValidForProjection())
{
Transform t = model.localTransform().inverse() * iter->second.transform();
cv::Vec3d rvec, tvec;
tvec.val[0] = t.x();
tvec.val[1] = t.y();
tvec.val[2] = t.z();
//cv::aruco::drawAxis(image, model.K(), model.D(), rvec, tvec, _preferencesDialog->getMarkerLength()<=0?0.1:_preferencesDialog->getMarkerLength() * 0.5f);
// In front of the camera?
if(t.z() > 0)
{
cv::Mat R;
t.rotationMatrix().convertTo(R, CV_64F);
cv::Rodrigues(R, rvec);
// project axis points
std::vector< cv::Point3f > axisPoints;
float length = _preferencesDialog->getMarkerLength()<=0?0.1:_preferencesDialog->getMarkerLength() * 0.5f;
axisPoints.push_back(cv::Point3f(0, 0, 0));
axisPoints.push_back(cv::Point3f(length, 0, 0));
axisPoints.push_back(cv::Point3f(0, length, 0));
axisPoints.push_back(cv::Point3f(0, 0, length));
std::vector< cv::Point2f > imagePoints;
projectPoints(axisPoints, rvec, tvec, model.K(), model.D(), imagePoints);
// draw axis lines
cv::line(image, imagePoints[0], imagePoints[1], cv::Scalar(0, 0, 255), 3);
cv::line(image, imagePoints[0], imagePoints[2], cv::Scalar(0, 255, 0), 3);
cv::line(image, imagePoints[0], imagePoints[3], cv::Scalar(255, 0, 0), 3);
cv::putText(image, uNumber2Str(-iter->first), imagePoints[0], cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 255, 255), 2);
//cv::aruco::drawAxis(image, model.K(), model.D(), rvec, tvec, _preferencesDialog->getMarkerLength()<=0?0.1:_preferencesDialog->getMarkerLength() * 0.5f);
// project axis points
std::vector< cv::Point3f > axisPoints;
float length = _preferencesDialog->getMarkerLength()<=0?0.1:_preferencesDialog->getMarkerLength() * 0.5f;
axisPoints.push_back(cv::Point3f(0, 0, 0));
axisPoints.push_back(cv::Point3f(length, 0, 0));
axisPoints.push_back(cv::Point3f(0, length, 0));
axisPoints.push_back(cv::Point3f(0, 0, length));
std::vector< cv::Point2f > imagePoints;
projectPoints(axisPoints, rvec, tvec, model.K(), model.D(), imagePoints);
//offset x based on camera index
bool valid = true;
if(i!=0)
{
if(model.imageWidth() <= 0)
{
valid = false;
UWARN("Cannot draw correctly landmark %d with provided camera model %d (missing image width)", -iter->first, (int)i);
}
else
{
for(int j=0; j<4; ++j)
{
imagePoints[j].x += i*model.imageWidth();
}
}
}
if(valid)
{
// draw axis lines
cv::line(image, imagePoints[0], imagePoints[1], cv::Scalar(0, 0, 255), 3);
cv::line(image, imagePoints[0], imagePoints[2], cv::Scalar(0, 255, 0), 3);
cv::line(image, imagePoints[0], imagePoints[3], cv::Scalar(255, 0, 0), 3);
cv::putText(image, uNumber2Str(-iter->first), imagePoints[0], cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 255, 255), 2);
}
}
}
}
}
}
guilib/src/PreferencesDialog.cpp
+2
View File
@@ -1130,6 +1130,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_identityGuess->setObjectName(Parameters::kRGBDLoopClosureIdentityGuess().c_str());
_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
_ui->loopClosure_bunlde->setObjectName(Parameters::kRGBDLocalBundleOnLoopClosure().c_str());
_ui->loopClosure_invertedReg->setObjectName(Parameters::kRGBDInvertedReg().c_str());
_ui->checkbox_rgbd_createOccupancyGrid->setObjectName(Parameters::kRGBDCreateOccupancyGrid().c_str());
_ui->RGBDMarkerDetection->setObjectName(Parameters::kRGBDMarkerDetection().c_str());
_ui->spinBox_maxOdomCacheSize->setObjectName(Parameters::kRGBDMaxOdomCacheSize().c_str());
@@ -1154,6 +1155,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str());
_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());
_ui->loopClosure_pnpRefineIterations->setObjectName(Parameters::kVisPnPRefineIterations().c_str());
_ui->loopClosure_pnpMaxVariance->setObjectName(Parameters::kVisPnPMaxVariance().c_str());
_ui->reextract_nn->setObjectName(Parameters::kVisCorNNType().c_str());
connect(_ui->reextract_nn, SIGNAL(currentIndexChanged(int)), this, SLOT(updateFeatureMatchingVisibility()));
_ui->reextract_nndrRatio->setObjectName(Parameters::kVisCorNNDR().c_str());
guilib/src/ui/preferencesDialog.ui
+356 -301
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-454</y>
<y>-853</y>
<width>756</width>
<height>3657</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>5</number>
<number>21</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -10929,10 +10929,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<string>Map Update</string>
</property>
<layout class="QGridLayout" name="gridLayout_47" columnstretch="0,1">
<item row="2" column="1">
<widget class="QLabel" name="label_432">
<item row="8" column="1">
<widget class="QLabel" name="label_scanMatching_14">
<property name="text">
<string>Maximum linear speed to update the map (0 means not limit).</string>
<string>Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used (assuming that registration strategy can deal with transformation estimation without guess).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -10942,16 +10942,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_scanMatching">
<property name="text">
<string>Neighbor link refining. When a new node is added to the graph, the transformation of its neighbor link (odometry) with the previous node is refined using ICP registration approach (laser scans required).</string>
<item row="17" column="0">
<widget class="QDoubleSpinBox" name="maxLocalizationDistance">
<property name="suffix">
<string> m</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
@@ -10968,16 +10965,61 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="13" column="0">
<widget class="QSpinBox" name="spinBox_maxLocalLocationsRetrieved"/>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="rgdb_angularUpdate">
<property name="suffix">
<string> rad</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>3.140000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="9" column="0">
<widget class="QCheckBox" name="loopClosure_reextract">
<item row="12" column="0">
<widget class="QCheckBox" name="memCovOffDiagIgnored">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="16" column="1">
<widget class="QLabel" name="label_space2">
<property name="text">
<string>Local radius for nodes selection in the local map. This parameter is used in some approaches of the sub-panels.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="15" column="0">
<widget class="QDoubleSpinBox" name="rgdb_localImmunizationRatio">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QDoubleSpinBox" name="rgdb_newMapOdomChange">
<property name="suffix">
@@ -10997,121 +11039,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="14" column="1">
<widget class="QLabel" name="label_scanMatching_5">
<property name="text">
<string>Ratio of working memory for which local nodes are immunized from transfer.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="rgbd_savedLocalizationIgnored">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_scanMatching_9">
<property name="text">
<string>Re-extract visual features when computing loop closure transformations. Raw features are not saved in database.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="rgdb_linearSpeedUpdate">
<property name="suffix">
<string> m/s</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QCheckBox" name="odomScanHistory">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="15" column="0">
<widget class="QDoubleSpinBox" name="localDetection_radius">
<property name="suffix">
<string> m</string>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="14" column="0">
<widget class="QDoubleSpinBox" name="rgdb_localImmunizationRatio">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QCheckBox" name="memCovOffDiagIgnored">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_163">
<property name="text">
<string>Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. </string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_433">
<property name="text">
<string>Maximum angular speed to update the map (0 means not limit).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="10" column="0">
<widget class="QCheckBox" name="loopClosure_bunlde">
<property name="text">
@@ -11119,45 +11046,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_scanMatching_10">
<property name="text">
<string>Do local bundle adjustment with neighborhood of the loop closure.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="11" column="1">
<widget class="QLabel" name="label_scanMatching_11">
<property name="text">
<string>Ignore off diagonal values of the odometry covariance matrix.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="rgdb_linearUpdate">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_153">
<property name="text">
@@ -11171,33 +11059,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="12" column="0">
<widget class="QCheckBox" name="rgbd_loopCovLimited">
<item row="2" column="1">
<widget class="QLabel" name="label_432">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="rgdb_angularUpdate">
<property name="suffix">
<string> rad</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>3.140000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_scanMatching_2">
<property name="text">
<string>If true, rtabmap will assume the robot is restarting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode.</string>
<string>Maximum linear speed to update the map (0 means not limit).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -11207,10 +11072,73 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="15" column="1">
<widget class="QLabel" name="label_space2">
<item row="4" column="0">
<widget class="QCheckBox" name="rgbd_savedLocalizationIgnored">
<property name="text">
<string>Local radius for nodes selection in the local map. This parameter is used in some approaches of the sub-panels.</string>
<string/>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="rgdb_linearSpeedUpdate">
<property name="suffix">
<string> m/s</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="rgdb_linearUpdate">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_163">
<property name="text">
<string>Odometry change detected that triggers a new map (0 means whatever the odometry change, the detector will still link the new pose in the current map). Also by default, when an odometry with Identity transformation is detected, a new map is automatically created. </string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="18" column="0">
<widget class="QSpinBox" name="spinBox_maxOdomCacheSize">
<property name="maximum">
<number>9999</number>
</property>
</widget>
</item>
<item row="16" column="0">
<widget class="QDoubleSpinBox" name="localDetection_radius">
<property name="suffix">
<string> m</string>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="12" column="1">
<widget class="QLabel" name="label_scanMatching_11">
<property name="text">
<string>Ignore off diagonal values of the odometry covariance matrix.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -11221,42 +11149,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</widget>
</item>
<item row="13" column="1">
<widget class="QLabel" name="label_scanMatching_3">
<property name="text">
<string>Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="17" column="0">
<widget class="QSpinBox" name="spinBox_maxOdomCacheSize">
<property name="maximum">
<number>9999</number>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="rgdb_angularSpeedUpdate">
<property name="suffix">
<string> rad/s</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>3.140000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="12" column="1">
<widget class="QLabel" name="label_scanMatching_12">
<property name="text">
<string>Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.</string>
@@ -11269,26 +11161,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="odomGravity">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="17" column="1">
<widget class="QLabel" name="label_space2_5">
<property name="text">
<string>Maximum odometry cache size. Used only in localization mode. This is used to get smoother localizations and to verify localization transforms (when maximum graph error is not null) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_scanMatching_7">
<property name="text">
@@ -11302,7 +11174,99 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="16" column="1">
<item row="9" column="1">
<widget class="QLabel" name="label_scanMatching_9">
<property name="text">
<string>Re-extract visual features when computing loop closure transformations. Raw features are not saved in database.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="rgdb_angularSpeedUpdate">
<property name="suffix">
<string> rad/s</string>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>3.140000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="13" column="0">
<widget class="QCheckBox" name="rgbd_loopCovLimited">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_433">
<property name="text">
<string>Maximum angular speed to update the map (0 means not limit).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="14" column="0">
<widget class="QSpinBox" name="spinBox_maxLocalLocationsRetrieved"/>
</item>
<item row="10" column="1">
<widget class="QLabel" name="label_scanMatching_10">
<property name="text">
<string>Do local bundle adjustment with neighborhood of the loop closure.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_scanMatching_2">
<property name="text">
<string>If true, rtabmap will assume the robot is restarting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QCheckBox" name="odomScanHistory">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QCheckBox" name="odomGravity">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="17" column="1">
<widget class="QLabel" name="label_space2_12">
<property name="text">
<string>Reject loop closures/localizations if the distance from the map is over this distance (0=disabled).</string>
@@ -11315,20 +11279,43 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="16" column="0">
<widget class="QDoubleSpinBox" name="maxLocalizationDistance">
<property name="suffix">
<string> m</string>
<item row="14" column="1">
<widget class="QLabel" name="label_scanMatching_3">
<property name="text">
<string>Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).</string>
</property>
<property name="value">
<double>1.000000000000000</double>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_scanMatching_14">
<item row="9" column="0">
<widget class="QCheckBox" name="loopClosure_reextract">
<property name="text">
<string>Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used (assuming that registration strategy can deal with transformation estimation without guess).</string>
<string/>
</property>
</widget>
</item>
<item row="18" column="1">
<widget class="QLabel" name="label_space2_5">
<property name="text">
<string>Maximum odometry cache size. Used only in localization mode. This is used to get smoother localizations and to verify localization transforms (when maximum graph error is not null) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="15" column="1">
<widget class="QLabel" name="label_scanMatching_5">
<property name="text">
<string>Ratio of working memory for which local nodes are immunized from transfer.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -11345,6 +11332,39 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_scanMatching">
<property name="text">
<string>Neighbor link refining. When a new node is added to the graph, the transformation of its neighbor link (odometry) with the previous node is refined using ICP registration approach (laser scans required).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="11" column="1">
<widget class="QLabel" name="label_scanMatching_16">
<property name="text">
<string>Inverted registration. On loop closure, do registration from the target to reference instead of reference to target.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QCheckBox" name="loopClosure_invertedReg">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@@ -18973,7 +18993,7 @@ Lower the ratio -&gt; higher the precision.</string>
<item>
<widget class="QStackedWidget" name="stackedWidget_loopClosureEstimation">
<property name="currentIndex">
<number>2</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_38">
<layout class="QVBoxLayout" name="verticalLayout_68">
@@ -19087,29 +19107,10 @@ Lower the ratio -&gt; higher the precision.</string>
<string>Motion Estimation: 3D to 2D (PnP)</string>
</property>
<layout class="QGridLayout" name="gridLayout_59" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_pnpReprojError">
<property name="suffix">
<string> pix</string>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="minimum">
<double>0.100000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>8.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_236">
<item row="2" column="1">
<widget class="QLabel" name="label_loopClosure_pnpOpenCV2">
<property name="text">
<string>Reprojection error.</string>
<string>Refine iterations.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -19138,10 +19139,10 @@ Lower the ratio -&gt; higher the precision.</string>
</item>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_235">
<item row="0" column="1">
<widget class="QLabel" name="label_236">
<property name="text">
<string>Flags.</string>
<string>Reprojection error.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -19151,16 +19152,22 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_loopClosure_pnpOpenCV2">
<property name="text">
<string>Refine iterations.</string>
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_pnpReprojError">
<property name="suffix">
<string> pix</string>
</property>
<property name="wordWrap">
<bool>true</bool>
<property name="decimals">
<number>1</number>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
<property name="minimum">
<double>0.100000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>8.000000000000000</double>
</property>
</widget>
</item>
@@ -19180,6 +19187,54 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_235">
<property name="text">
<string>Flags.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_loopClosure_pnpOpenCV2_2">
<property name="text">
<string>Max linear variance between 3D point correspondences after PnP. 0 means disabled.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="loopClosure_pnpMaxVariance">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>4</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>99.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
</layout>
</widget>
</item>