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

Added Vis/PnPSplitLinearCovComponents parameter (default false -> same as before)

Browse Source
This commit is contained in:
matlabbe
2024-02-07 14:43:16 -08:00
parent 1dadd50cf2
commit 510aef19e4
11 changed files with 259 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Parameters.h
+1
View File
@@ -671,6 +671,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Vis, PnPVarianceMedianRatio, int, 4, uFormat("[%s = 1] Ratio used to compute variance of the estimated transformation if 3D correspondences are provided (should be > 1). The higher it is, the smaller the covariance will be. With accurate depth estimation, this could be set to 2. For depth estimated by stereo, 4 or more maybe used to ignore large errors of very far points.", kVisEstimationType().c_str()));
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, PnPSamplingPolicy, unsigned int, 1, uFormat("[%s = 1] Multi-camera random sampling policy: 0=AUTO, 1=ANY, 2=HOMOGENEOUS. With HOMOGENEOUS policy, RANSAC will be done uniformly against all cameras, so at least 2 matches per camera are required. With ANY policy, RANSAC is not constraint to sample on all cameras at the same time. AUTO policy will use HOMOGENEOUS if there are at least 2 matches per camera, otherwise it will fallback to ANY policy.", kVisEstimationType().c_str()).c_str());
RTABMAP_PARAM(Vis, PnPSplitLinearCovComponents, bool, false, uFormat("[%s = 1] Compute variance for each linear component instead of using the combined XYZ variance for all linear components.", kVisEstimationType().c_str()).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
@@ -84,6 +84,7 @@ private:
int _PnPRefineIterations;
int _PnPVarMedianRatio;
float _PnPMaxVar;
bool _PnPSplitLinearCovarianceComponents;
unsigned int _multiSamplingPolicy;
int _correspondencesApproach;
int _flowWinSize;
corelib/include/rtabmap/core/util3d_motion_estimation.h
+4 -2
View File
@@ -54,7 +54,8 @@ Transform RTABMAP_CORE_EXPORT estimateMotion3DTo2D(
const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
cv::Mat * covariance = 0, // mean reproj error if words3B is not set
std::vector<int> * matchesOut = 0,
std::vector<int> * inliersOut = 0);
std::vector<int> * inliersOut = 0,
bool splitLinearCovarianceComponents = false);
Transform RTABMAP_CORE_EXPORT estimateMotion3DTo2D(
const std::map<int, cv::Point3f> & words3A,
@@ -72,7 +73,8 @@ Transform RTABMAP_CORE_EXPORT estimateMotion3DTo2D(
const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
cv::Mat * covariance = 0, // mean reproj error if words3B is not set
std::vector<int> * matchesOut = 0,
std::vector<int> * inliersOut = 0);
std::vector<int> * inliersOut = 0,
bool splitLinearCovarianceComponents = false);
Transform RTABMAP_CORE_EXPORT estimateMotion3DTo3D(
const std::map<int, cv::Point3f> & words3A,
corelib/src/Memory.cpp
+1 -1
View File
@@ -3372,7 +3372,7 @@ bool Memory::addLink(const Link & link, bool addInDatabase)
{
UASSERT(link.type() > Link::kNeighbor && link.type() != Link::kUndef);
ULOGGER_INFO("to=%d, from=%d transform: %s var=%f", link.to(), link.from(), link.transform().prettyPrint().c_str(), link.transVariance());
ULOGGER_INFO("to=%d, from=%d transform: %s var=%f", link.to(), link.from(), link.transform().prettyPrint().c_str(), link.transVariance(false));
Signature * toS = _getSignature(link.to());
Signature * fromS = _getSignature(link.from());
if(toS && fromS)
corelib/src/Odometry.cpp
+9
View File
@@ -335,6 +335,15 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
}
if(!data.imageRaw().empty())
{
UDEBUG("Processing image data %dx%d: rgbd models=%ld, stereo models=%ld",
data.imageRaw().cols,
data.imageRaw().rows,
data.cameraModels().size(),
data.stereoCameraModels().size());
}
if(!_imagesAlreadyRectified && !this->canProcessRawImages() && !data.imageRaw().empty())
{
corelib/src/RegistrationVis.cpp
+7 -2
View File
@@ -72,6 +72,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
_PnPVarMedianRatio(Parameters::defaultVisPnPVarianceMedianRatio()),
_PnPMaxVar(Parameters::defaultVisPnPMaxVariance()),
_PnPSplitLinearCovarianceComponents(Parameters::defaultVisPnPSplitLinearCovComponents()),
_multiSamplingPolicy(Parameters::defaultVisPnPSamplingPolicy()),
_correspondencesApproach(Parameters::defaultVisCorType()),
_flowWinSize(Parameters::defaultVisCorFlowWinSize()),
@@ -130,6 +131,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations);
Parameters::parse(parameters, Parameters::kVisPnPVarianceMedianRatio(), _PnPVarMedianRatio);
Parameters::parse(parameters, Parameters::kVisPnPMaxVariance(), _PnPMaxVar);
Parameters::parse(parameters, Parameters::kVisPnPSplitLinearCovComponents(), _PnPSplitLinearCovarianceComponents);
Parameters::parse(parameters, Parameters::kVisPnPSamplingPolicy(), _multiSamplingPolicy);
Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
@@ -295,6 +297,7 @@ Transform RegistrationVis::computeTransformationImpl(
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=%f", Parameters::kVisPnPSplitLinearCovComponents().c_str(), _PnPSplitLinearCovarianceComponents);
UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
@@ -1637,7 +1640,8 @@ Transform RegistrationVis::computeTransformationImpl(
words3B,
&covariances[dir],
&matchesV,
&inliersV);
&inliersV,
_PnPSplitLinearCovarianceComponents);
inliers[dir] = inliersV;
matches[dir] = matchesV;
}
@@ -1660,7 +1664,8 @@ Transform RegistrationVis::computeTransformationImpl(
words3B,
&covariances[dir],
&matchesV,
&inliersV);
&inliersV,
_PnPSplitLinearCovarianceComponents);
inliers[dir] = inliersV;
matches[dir] = matchesV;
}
corelib/src/Rtabmap.cpp
+7 -4
View File
@@ -1442,8 +1442,10 @@ bool Rtabmap::process(
float angleToClosestNodeInTheGraph = 0;
if(_rgbdSlamMode)
{
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(0,0));
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(5,5));
double linVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at<double>(0,0), odomCovariance.at<double>(1,1)>=9999?0:odomCovariance.at<double>(1,1), odomCovariance.at<double>(2,2)>=9999?0:odomCovariance.at<double>(2,2));
double angVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at<double>(3,3)>=9999?0:odomCovariance.at<double>(3,3), odomCovariance.at<double>(4,4)>=9999?0:odomCovariance.at<double>(4,4), odomCovariance.at<double>(5,5));
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), (float)linVar);
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), (float)angVar);
//Verify if there was a rehearsal
int rehearsedId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
@@ -2959,9 +2961,10 @@ bool Rtabmap::process(
{
// Make the new one the parent of the old one
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
loopClosureLinearVariance = uMax3(info.covariance.at<double>(0,0), info.covariance.at<double>(1,1)>=9999?0:info.covariance.at<double>(1,1), info.covariance.at<double>(2,2)>=9999?0:info.covariance.at<double>(2,2));
loopClosureAngularVariance = uMax3(info.covariance.at<double>(3,3)>=9999?0:info.covariance.at<double>(3,3), info.covariance.at<double>(4,4)>=9999?0:info.covariance.at<double>(4,4), info.covariance.at<double>(5,5));
cv::Mat information = getInformation(info.covariance);
loopClosureLinearVariance = 1.0/information.at<double>(0,0);
loopClosureAngularVariance = 1.0/information.at<double>(5,5);
rejectedGlobalLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
if(!rejectedGlobalLoopClosure)
{
corelib/src/util3d_motion_estimation.cpp
+113 -18
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UMath.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_correspondences.h"
@@ -70,7 +71,8 @@ Transform estimateMotion3DTo2D(
const std::map<int, cv::Point3f> & words3B,
cv::Mat * covariance,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut)
std::vector<int> * inliersOut,
bool splitLinearCovarianceComponents)
{
UASSERT(cameraModel.isValidForProjection());
UASSERT(!guess.isNull());
@@ -155,25 +157,36 @@ Transform estimateMotion3DTo2D(
if(covariance && (!words3B.empty() || cameraModel.imageSize() != cv::Size()))
{
std::vector<float> errorSqrdDists(inliers.size());
std::vector<float> errorSqrdX;
std::vector<float> errorSqrdY;
std::vector<float> errorSqrdZ;
if(splitLinearCovarianceComponents)
{
errorSqrdX.resize(inliers.size());
errorSqrdY.resize(inliers.size());
errorSqrdZ.resize(inliers.size());
}
std::vector<float> errorSqrdAngles(inliers.size());
Transform localTransformInv = cameraModel.localTransform().inverse();
Transform transformCameraFrameInv = (transform * cameraModel.localTransform()).inverse();
Transform transformCameraFrame = transform * cameraModel.localTransform();
Transform transformCameraFrameInv = transformCameraFrame.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
// Get 3D point from cameraB base frame in cameraA base frame
std::map<int, cv::Point3f>::const_iterator iter = words3B.find(matches[inliers[i]]);
cv::Point3f newPt;
if(iter!=words3B.end() && util3d::isFinite(iter->second))
{
newPt = util3d::transformPoint(iter->second, localTransformInv);
newPt = util3d::transformPoint(iter->second, transform);
}
else
{
// Project obj point from base frame of cameraA in cameraB frame (z+ in front of the cameraB)
cv::Point3f objPtCamBFrame = util3d::transformPoint(objPt, transformCameraFrameInv);
//compute from projection
Eigen::Vector3f ray = projectDepthTo3DRay(
cameraModel.imageSize(),
@@ -184,7 +197,20 @@ Transform estimateMotion3DTo2D(
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
newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPtCamBFrame.z*1.1; // Add 10 % error
//transform back into cameraA base frame
newPt = util3d::transformPoint(newPt, transformCameraFrame);
}
if(splitLinearCovarianceComponents)
{
double errorX = objPt.x-newPt.x;
double errorY = objPt.y-newPt.y;
double errorZ = objPt.z-newPt.z;
errorSqrdX[i] = errorX * errorX;
errorSqrdY[i] = errorY * errorY;
errorSqrdZ[i] = errorZ * errorZ;
}
errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
@@ -204,6 +230,25 @@ Transform estimateMotion3DTo2D(
UASSERT(uIsFinite(median_error_sqr_ang));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;
if(splitLinearCovarianceComponents)
{
std::sort(errorSqrdX.begin(), errorSqrdX.end());
double median_error_sqr_x = 2.1981 * (double)errorSqrdX[errorSqrdX.size () / varianceMedianRatio];
std::sort(errorSqrdY.begin(), errorSqrdY.end());
double median_error_sqr_y = 2.1981 * (double)errorSqrdY[errorSqrdY.size () / varianceMedianRatio];
std::sort(errorSqrdZ.begin(), errorSqrdZ.end());
double median_error_sqr_z = 2.1981 * (double)errorSqrdZ[errorSqrdZ.size () / varianceMedianRatio];
UASSERT(uIsFinite(median_error_sqr_x));
UASSERT(uIsFinite(median_error_sqr_y));
UASSERT(uIsFinite(median_error_sqr_z));
covariance->at<double>(0,0) = median_error_sqr_x;
covariance->at<double>(1,1) = median_error_sqr_y;
covariance->at<double>(2,2) = median_error_sqr_z;
median_error_sqr_lin = uMax3(median_error_sqr_x, median_error_sqr_y, median_error_sqr_z);
}
if(maxVariance > 0 && median_error_sqr_lin > maxVariance)
{
UWARN("Rejected PnP transform, variance is too high! %f > %f!", median_error_sqr_lin, maxVariance);
@@ -259,7 +304,8 @@ Transform estimateMotion3DTo2D(
const std::map<int, cv::Point3f> & words3B,
cv::Mat * covariance,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut)
std::vector<int> * inliersOut,
bool splitLinearCovarianceComponents)
{
Transform transform;
#ifndef RTABMAP_OPENGV
@@ -491,27 +537,44 @@ Transform estimateMotion3DTo2D(
// compute variance (like in PCL computeVariance() method of sac_model.h)
if(covariance)
{
std::vector<float> errorSqrdX;
std::vector<float> errorSqrdY;
std::vector<float> errorSqrdZ;
if(splitLinearCovarianceComponents)
{
errorSqrdX.resize(inliers.size());
errorSqrdY.resize(inliers.size());
errorSqrdZ.resize(inliers.size());
}
std::vector<float> errorSqrdDists(inliers.size());
std::vector<float> errorSqrdAngles(inliers.size());
std::vector<Transform> transformsCameraFrame(cameraModels.size());
std::vector<Transform> transformsCameraFrameInv(cameraModels.size());
for(size_t i=0; i<cameraModels.size(); ++i)
{
transformsCameraFrame[i] = transform * cameraModels[i].localTransform();
transformsCameraFrameInv[i] = transformsCameraFrame[i].inverse();
}
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
// Get 3D point from cameraB base frame in cameraA base frame
std::map<int, cv::Point3f>::const_iterator iter = words3B.find(matches[inliers[i]]);
cv::Point3f newPt;
if(iter!=words3B.end() && util3d::isFinite(iter->second))
{
newPt = util3d::transformPoint(iter->second, cameraModels[cameraIndex].localTransform().inverse());
newPt = util3d::transformPoint(iter->second, transform);
}
else
{
int cameraIndex = cameraIndexes[inliers[i]];
// Project obj point from base frame of cameraA in cameraB frame (z+ in front of the cameraB)
cv::Point3f objPtCamBFrame = util3d::transformPoint(objPt, transformsCameraFrameInv[cameraIndex]);
//compute from projection
Eigen::Vector3f ray = projectDepthTo3DRay(
cameraModels[cameraIndex].imageSize(),
@@ -522,7 +585,20 @@ Transform estimateMotion3DTo2D(
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
newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPtCamBFrame.z*1.1; // Add 10 % error
//transfor back into cameraA base frame
newPt = util3d::transformPoint(newPt, transformsCameraFrame[cameraIndex]);
}
if(splitLinearCovarianceComponents)
{
double errorX = objPt.x-newPt.x;
double errorY = objPt.y-newPt.y;
double errorZ = objPt.z-newPt.z;
errorSqrdX[i] = errorX * errorX;
errorSqrdY[i] = errorY * errorY;
errorSqrdZ[i] = errorZ * errorZ;
}
errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
@@ -542,6 +618,25 @@ Transform estimateMotion3DTo2D(
UASSERT(uIsFinite(median_error_sqr_ang));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;
if(splitLinearCovarianceComponents)
{
std::sort(errorSqrdX.begin(), errorSqrdX.end());
double median_error_sqr_x = 2.1981 * (double)errorSqrdX[errorSqrdX.size () / varianceMedianRatio];
std::sort(errorSqrdY.begin(), errorSqrdY.end());
double median_error_sqr_y = 2.1981 * (double)errorSqrdY[errorSqrdY.size () / varianceMedianRatio];
std::sort(errorSqrdZ.begin(), errorSqrdZ.end());
double median_error_sqr_z = 2.1981 * (double)errorSqrdZ[errorSqrdZ.size () / varianceMedianRatio];
UASSERT(uIsFinite(median_error_sqr_x));
UASSERT(uIsFinite(median_error_sqr_y));
UASSERT(uIsFinite(median_error_sqr_z));
covariance->at<double>(0,0) = median_error_sqr_x;
covariance->at<double>(1,1) = median_error_sqr_y;
covariance->at<double>(2,2) = median_error_sqr_z;
median_error_sqr_lin = uMax3(median_error_sqr_x, median_error_sqr_y, median_error_sqr_z);
}
if(maxVariance > 0 && median_error_sqr_lin > maxVariance)
{
UWARN("Rejected PnP transform, variance is too high! %f > %f!", median_error_sqr_lin, maxVariance);
guilib/src/MainWindow.cpp
+6 -4
View File
@@ -1775,6 +1775,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
//Process info
if(_preferencesDialog->isCacheSavedInFigures() || _ui->statsToolBox->isVisible())
{
double linVar = uMax3(odom.info().reg.covariance.at<double>(0,0), odom.info().reg.covariance.at<double>(1,1)>=9999?0:odom.info().reg.covariance.at<double>(1,1), odom.info().reg.covariance.at<double>(2,2)>=9999?0:odom.info().reg.covariance.at<double>(2,2));
double angVar = uMax3(odom.info().reg.covariance.at<double>(3,3)>=9999?0:odom.info().reg.covariance.at<double>(3,3), odom.info().reg.covariance.at<double>(4,4)>=9999?0:odom.info().reg.covariance.at<double>(4,4), odom.info().reg.covariance.at<double>(5,5));
_ui->statsToolBox->updateStat("Odometry/Inliers/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliers, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/InliersMeanDistance/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersMeanDistance, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/InliersDistribution/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersDistribution, _preferencesDialog->isCacheSavedInFigures());
@@ -1788,10 +1790,10 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_ui->statsToolBox->updateStat("Odometry/ICPRMS/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpRMS, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/Matches/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.matches, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/MatchesRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().features<=0?0.0f:float(odom.info().reg.matches)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevLin/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)odom.info().reg.covariance.at<double>(0,0)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.covariance.at<double>(0,0), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevAng/rad", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)odom.info().reg.covariance.at<double>(5,5)), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.covariance.at<double>(5,5), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevLin/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)linVar), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceLin/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)linVar, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/StdDevAng/rad", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), sqrt((float)angVar), _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/VarianceAng/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)angVar, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().timeEstimation*1000.0f, _preferencesDialog->isCacheSavedInFigures());
if(odom.info().timeParticleFiltering>0.0f)
{
guilib/src/PreferencesDialog.cpp
+1
View File
@@ -1190,6 +1190,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_pnpVarMedianRatio->setObjectName(Parameters::kVisPnPVarianceMedianRatio().c_str());
_ui->loopClosure_pnpMaxVariance->setObjectName(Parameters::kVisPnPMaxVariance().c_str());
_ui->loopClosure_pnpSamplingPolicy->setObjectName(Parameters::kVisPnPSamplingPolicy().c_str());
_ui->loopClosure_pnpSplitLinearCovComponents->setObjectName(Parameters::kVisPnPSplitLinearCovComponents().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
+109 -89
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-1092</y>
<width>713</width>
<height>3933</height>
</rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>8</number>
<number>21</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -21292,10 +21292,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="1" column="1">
<widget class="QLabel" name="label_235">
<item row="0" column="1">
<widget class="QLabel" name="label_655">
<property name="text">
<string>Flags.</string>
<string>Reprojection error.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -21305,25 +21305,6 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QComboBox" name="loopClosure_pnpSamplingPolicy">
<item>
<property name="text">
<string>AUTO</string>
</property>
</item>
<item>
<property name="text">
<string>ANY</string>
</property>
</item>
<item>
<property name="text">
<string>HOMOGENEOUS</string>
</property>
</item>
</widget>
</item>
<item row="1" column="0">
<widget class="QComboBox" name="loopClosure_pnpFlags">
<item>
@@ -21343,45 +21324,67 @@ Lower the ratio -&gt; higher the precision.</string>
</item>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_loopClosure_pnpOpenCV2">
<property name="text">
<string>Refine iterations.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
<item row="5" column="0">
<widget class="QComboBox" name="loopClosure_pnpSamplingPolicy">
<item>
<property name="text">
<string>AUTO</string>
</property>
</item>
<item>
<property name="text">
<string>ANY</string>
</property>
</item>
<item>
<property name="text">
<string>HOMOGENEOUS</string>
</property>
</item>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_655">
<property name="text">
<string>Reprojection error.</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>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="loopClosure_pnpRefineIterations">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>999999</number>
<double>0.100000000000000</double>
</property>
<property name="singleStep">
<number>1</number>
<double>1.000000000000000</double>
</property>
<property name="value">
<number>1</number>
<double>8.000000000000000</double>
</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_4">
<property name="text">
<string>Ratio used to compute variance of the estimated transformation if 3D correspondences are provided (should be &gt; 1). The higher it is, the smaller the covariance will be. With accurate depth estimation, this could be set to 2. For depth estimated by stereo, 4 or more maybe used to ignore large errors of very far points.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
@@ -21407,25 +21410,6 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<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="5" column="1">
<widget class="QLabel" name="label_loopClosure_pnpOpenCV2_3">
<property name="text">
@@ -21439,19 +21423,6 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_656">
<property name="text">
<string>Reprojection error.</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_loopClosure_pnpOpenCV2_2">
<property name="text">
@@ -21465,10 +21436,10 @@ Lower the ratio -&gt; higher the precision.</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_loopClosure_pnpOpenCV2_4">
<item row="0" column="1">
<widget class="QLabel" name="label_656">
<property name="text">
<string>Ratio used to compute variance of the estimated transformation if 3D correspondences are provided (should be &gt; 1). The higher it is, the smaller the covariance will be. With accurate depth estimation, this could be set to 2. For depth estimated by stereo, 4 or more maybe used to ignore large errors of very far points.</string>
<string>Reprojection error.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -21491,6 +21462,55 @@ 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>
</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="QSpinBox" name="loopClosure_pnpRefineIterations">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>1</number>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_loopClosure_pnpOpenCV2_5">
<property name="text">
<string>Compute variance for each linear component instead of using the combined XYZ variance for all linear components.</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="loopClosure_pnpSplitLinearCovComponents">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</item>