i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed PnP in OpenCV3: added parameter Vis/PnPRefineIterations (default 1)

Browse Source 
CameraRGBDImages: Fixed calibration not loaded
Fixed loading ground truth from RGBD-SLAM format
MainWindow: added ground truth paths in CloudViewer and GraphViewer
This commit is contained in:
matlabbe
2015-12-22 19:32:52 -05:00
parent 2e9634cf65
commit 51300dde06
25 changed files with 986 additions and 917 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Odometry.h
+2 -2
View File
@@ -67,7 +67,7 @@ public:
int getEstimationType() const {return _estimationType;}
double getPnPReprojError() const {return _pnpReprojError;}
int getPnPFlags() const {return _pnpFlags;}
bool getPnPOpenCV2() const {return _pnpOpenCV2;}
int getPnPRefineIterations() const {return _pnpRefineIterations;}
const Transform & previousTransform() const {return previousTransform_;}
bool isVarianceFromInliersCount() const {return _varianceFromInliersCount;}
@@ -98,7 +98,7 @@ private:
int _estimationType;
double _pnpReprojError;
int _pnpFlags;
bool _pnpOpenCV2;
int _pnpRefineIterations;
bool _varianceFromInliersCount;
float _kalmanProcessNoise;
float _kalmanMeasurementNoise;
corelib/include/rtabmap/core/Parameters.h
+2 -2
View File
@@ -364,12 +364,12 @@ class RTABMAP_EXP Parameters
// Visual registration parameters
RTABMAP_PARAM(Vis, EstimationType, int, 0, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
RTABMAP_PARAM(Vis, ForwardEstOnly, bool, true, "Forward estimation (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
RTABMAP_PARAM(Vis, ForwardEstOnly, bool, true, "Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
RTABMAP_PARAM(Vis, InlierDistance, float, 0.1, "[Vis/EstimationType = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.");
RTABMAP_PARAM(Vis, RefineIterations, int, 10, "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
RTABMAP_PARAM(Vis, PnPReprojError, double, 5.0, "[Vis/EstimationType = 1] PnP reprojection error.");
RTABMAP_PARAM(Vis, PnPFlags, int, 1, "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
RTABMAP_PARAM(Vis, PnPOpenCV2, bool, true, "[Vis/EstimationType = 1] Use OpenCV2 solvePnPRansac() in OpenCV3.");
RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, "[Vis/EstimationType = 1] Refine iterations.");
RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.02, "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
RTABMAP_PARAM(Vis, MinInliers, int, 10, "Minimum feature correspondences to compute/accept the transformation.");
RTABMAP_PARAM(Vis, Iterations, int, 100, "Maximum iterations to compute the transform.");
corelib/include/rtabmap/core/RegistrationVis.h
+1 -1
View File
@@ -67,7 +67,7 @@ private:
bool _forwardEstimateOnly;
double _PnPReprojError;
int _PnPFlags;
bool _PnPOpenCV2;
bool _PnPRefineIterations;
int _correspondencesApproach;
int _flowWinSize;
int _flowIterations;
corelib/include/rtabmap/core/util3d_features.h
+1 -1
View File
@@ -73,7 +73,7 @@ std::map<int, cv::Point3f> RTABMAP_EXP generateWords3DMono(
int pnpIterations = 100,
float pnpReprojError = 8.0f,
int pnpFlags = 0, // cv::SOLVEPNP_ITERATIVE
bool pnpOpenCV2 = true,
int pnpRefineIterations = 1,
float ransacParam1 = 3.0f,
float ransacParam2 = 0.99f,
const std::map<int, cv::Point3f> & refGuess3D = std::map<int, cv::Point3f>(),
corelib/include/rtabmap/core/util3d_motion_estimation.h
+10 -9
View File
@@ -47,7 +47,7 @@ Transform RTABMAP_EXP estimateMotion3DTo2D(
int iterations = 100,
double reprojError = 5.,
int flagsPnP = 0,
bool pnpOpenCV2 = true,
int pnpRefineIterations = 1,
const Transform & guess = Transform::getIdentity(),
const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
double * varianceOut = 0, // mean reproj error if words3B is not set
@@ -66,19 +66,20 @@ Transform RTABMAP_EXP estimateMotion3DTo3D(
std::vector<int> * inliersOut = 0);
void RTABMAP_EXP solvePnPRansac(
cv::InputArray _opoints,
cv::InputArray _ipoints,
cv::InputArray _cameraMatrix,
cv::InputArray _distCoeffs,
cv::OutputArray _rvec,
cv::OutputArray _tvec,
const std::vector<cv::Point3f> & objectPoints,
const std::vector<cv::Point2f> & imagePoints,
const cv::Mat & cameraMatrix,
const cv::Mat & distCoeffs,
cv::Mat & rvec,
cv::Mat & tvec,
bool useExtrinsicGuess,
int iterationsCount,
float reprojectionError,
int minInliersCount,
cv::OutputArray _inliers,
std::vector<int> & inliers,
int flags,
bool opencv2version);
int refineIterations = 1,
float refineSigma = 3.0f);
} // namespace util3d
} // namespace rtabmap
corelib/include/rtabmap/core/util3d_registration.h
+1 -2
View File
@@ -50,9 +50,8 @@ Transform RTABMAP_EXP transformFromXYZCorrespondences(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
double inlierThreshold = 0.02,
int iterations = 100,
bool refineModel = false,
double refineModelSigma = 3.0,
int refineModelIterations = 10,
double refineModelSigma = 3.0,
std::vector<int> * inliers = 0,
double * variance = 0);
corelib/src/CameraRGB.cpp
+16 -3
View File
@@ -174,6 +174,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
}
// look for calibration files
UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
if(!calibrationFolder.empty() && !cameraName.empty())
{
if(!_model.load(calibrationFolder, cameraName))
@@ -285,7 +286,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
else if(_groundTruthFormat == 1)
{
//Match ground truth values with images
groundTruth_.resize(stamps_.size(), Transform());
groundTruth_.clear();
std::map<double, int> stampsToIds;
for(std::map<int, double>::iterator iter=stamps.begin(); iter!=stamps.end(); ++iter)
{
@@ -293,6 +294,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
}
std::vector<double> values = uValues(stamps);
Transform firstPoseInv;
for(std::list<double>::iterator ster=stamps_.begin(); ster!=stamps_.end(); ++ster)
{
Transform pose; // null transform
@@ -319,7 +321,18 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
pose = ta.interpolate(t, tb);
}
}
}
if(!pose.isNull())
{
if(firstPoseInv.isNull())
{
firstPoseInv = pose.inverse();
pose.setIdentity();
}
else
{
pose = firstPoseInv * pose;
}
}
groundTruth_.push_back(pose);
}
@@ -328,7 +341,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
{
groundTruth_ = uValuesList(poses);
}
UASSERT(groundTruth_.size() == stamps_.size());
UASSERT_MSG(groundTruth_.size() == stamps_.size(), uFormat("%d vs %d", (int)groundTruth_.size(), (int)stamps_.size()).c_str());
}
}
corelib/src/CameraRGBD.cpp
+1 -1
View File
@@ -1690,7 +1690,7 @@ CameraRGBDImages::~CameraRGBDImages()
bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::string & cameraName)
{
bool success = false;
if(CameraImages::init() && cameraDepth_.init())
if(CameraImages::init(calibrationFolder, cameraName) && cameraDepth_.init())
{
if(this->imagesCount() == cameraDepth_.imagesCount())
{
corelib/src/Graph.cpp
+7 -1
View File
@@ -202,7 +202,7 @@ bool importPoses(
std::list<std::string> strList = uSplit(str);
if(strList.size() == 8)
{
double stamp = uStr2Float(strList.front());
double stamp = uStr2Double(strList.front());
strList.pop_front();
str = uJoin(strList, " ");
Transform pose = Transform::fromString(str);
@@ -214,6 +214,12 @@ bool importPoses(
{
stamps->insert(std::make_pair(id, stamp));
}
// we need to remove optical rotation
// z pointing front, x left, y down
Transform t( 0, 0, 1, 0,
-1, 0, 0, 0,
0,-1, 0, 0);
pose = t * pose * t.inverse();
poses.insert(std::make_pair(id, pose));
}
else
corelib/src/Memory.cpp
+2 -1
View File
@@ -2040,7 +2040,7 @@ Transform Memory::computeVisualTransform(
tmpTo.setWords3(std::multimap<int, cv::Point3f>());
transform = _registrationVis->computeTransformation(tmpFrom, tmpTo, Transform::getIdentity(), rejectedMsg, &inliersV, variance);
}
else
else if(fromS->getWords().size() && toS->getWords().size())
{
transform = _registrationVis->computeTransformation(*fromS, *toS, Transform::getIdentity(), rejectedMsg, &inliersV, variance);
}
@@ -3310,6 +3310,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
s->sensorData().setLaserScanRaw(laserScan, maxLaserScanMaxPts, data.laserScanMaxRange());
s->sensorData().setUserDataRaw(data.userDataRaw());
}
s->sensorData().setGroundTruth(data.groundTruth());
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);
corelib/src/Odometry.cpp
+2 -2
View File
@@ -55,7 +55,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_estimationType(Parameters::defaultVisEstimationType()),
_pnpReprojError(Parameters::defaultVisPnPReprojError()),
_pnpFlags(Parameters::defaultVisPnPFlags()),
_pnpOpenCV2(Parameters::defaultVisPnPOpenCV2()),
_pnpRefineIterations(Parameters::defaultVisPnPRefineIterations()),
_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount()),
_kalmanProcessNoise(Parameters::defaultOdomKalmanProcessNoise()),
_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
@@ -79,7 +79,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _pnpReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _pnpFlags);
Parameters::parse(parameters, Parameters::kVisPnPOpenCV2(), _pnpOpenCV2);
Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _pnpRefineIterations);
UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
corelib/src/OdometryBOW.cpp
+1 -1
View File
@@ -257,7 +257,7 @@ Transform OdometryBOW::computeTransform(
this->getIterations(),
this->getPnPReprojError(),
this->getPnPFlags(),
this->getPnPOpenCV2(),
this->getPnPRefineIterations(),
this->getPose(),
uMultimapToMap(newSignature->getWords3()),
isVarianceFromInliersCount()?0:&variance, // don't compute variance if we use inliers
corelib/src/OdometryF2F.cpp
+1 -1
View File
@@ -84,7 +84,7 @@ Transform OdometryF2F::computeTransform(
output = registration_.computeTransformationMod(
refFrame_,
newFrame,
guessFromMotion_?motionSinceLastKeyFrame_*this->previousTransform():Transform::getIdentity(),
guessFromMotion_?motionSinceLastKeyFrame_*this->previousTransform():Transform(),
&rejectedMsg,
&inliers,
&variance);
corelib/src/OdometryMono.cpp
+3 -3
View File
@@ -368,7 +368,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
0, // min inliers
inliersV,
this->getPnPFlags(),
this->getPnPOpenCV2());
this->getPnPRefineIterations());
UDEBUG("inliers=%d/%d", (int)inliersV.size(), (int)objectPoints.size());
@@ -434,7 +434,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
this->getIterations(),
this->getPnPReprojError(),
this->getPnPFlags(),
this->getPnPOpenCV2(),
this->getPnPRefineIterations(),
fundMatrixReprojError_,
fundMatrixConfidence_,
previousGuess,
@@ -897,7 +897,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
0, // min inliers
inliersPnP,
this->getPnPFlags(),
this->getPnPOpenCV2());
this->getPnPRefineIterations());
UDEBUG("PnP inliers = %d / %d", (int)inliersPnP.size(), (int)objectPoints.size());
corelib/src/RegistrationVis.cpp
+27 -4
View File
@@ -52,7 +52,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
_forwardEstimateOnly(Parameters::defaultVisForwardEstOnly()),
_PnPReprojError(Parameters::defaultVisPnPReprojError()),
_PnPFlags(Parameters::defaultVisPnPFlags()),
_PnPOpenCV2(Parameters::defaultVisPnPOpenCV2()),
_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
_correspondencesApproach(Parameters::defaultVisCorType()),
_flowWinSize(Parameters::defaultVisCorFlowWinSize()),
_flowIterations(Parameters::defaultVisCorFlowIterations()),
@@ -96,7 +96,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
Parameters::parse(parameters, Parameters::kVisPnPOpenCV2(), _PnPOpenCV2);
Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations);
Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
@@ -557,7 +557,7 @@ Transform RegistrationVis::computeTransformationMod(
_iterations,
_PnPReprojError,
_PnPFlags, // cv::SOLVEPNP_ITERATIVE
_PnPOpenCV2,
_PnPRefineIterations,
1.0f,
0.99f,
uMultimapToMapUnique(signatureA->getWords3()), // for scale estimation
@@ -635,7 +635,7 @@ Transform RegistrationVis::computeTransformationMod(
_iterations,
_PnPReprojError,
_PnPFlags,
_PnPOpenCV2,
_PnPRefineIterations,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
uMultimapToMapUnique(signatureA->getWords3()),
_varianceFromInliersCount?0:&variances[dir],
@@ -717,6 +717,28 @@ Transform RegistrationVis::computeTransformationMod(
}
}
else
{
/*if(!guess.isNull())
{
// use the transform nearest of the guess
int index = 0;
if(transforms[0].getDistance(guess) > transforms[1].getDistance(guess))
{
index = 1;
}
transform = transforms[index];
if(inliersOut)
{
*inliersOut = inliers[index];
}
variance = variances[index];
if(_varianceFromInliersCount)
{
variance = inliers[index].size() > 0?1.0f/float(inliers[index].size()):1.0f;
}
}
else*/
{
transform = transforms[0].interpolate(0.5f, transforms[1]);
if(inliersOut)
@@ -731,6 +753,7 @@ Transform RegistrationVis::computeTransformationMod(
}
}
}
}
else
{
transform = transforms[0];
corelib/src/util2d.cpp
+14 -8
View File
@@ -167,7 +167,6 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
int tmpMaxDisparity = maxDisparity;
int iterations = 0;
std::vector<float> scores;
for(int level=maxLevel; level>=0; --level)
{
UASSERT(level < (int)leftPyramid.size());
@@ -199,7 +198,13 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
localMinDisparity += maxCol-leftPyramid[level].cols-1;
}
scores = std::vector<float>(localMinDisparity-localMaxDisparity+1, 0.0f);
if(localMinDisparity < localMaxDisparity)
{
localMaxDisparity = localMinDisparity;
}
int length = localMinDisparity-localMaxDisparity+1;
std::vector<float> scores = std::vector<float>(length, 0.0f);
for(int d=localMinDisparity; d>localMaxDisparity; --d)
{
++iterations;
@@ -220,6 +225,7 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
{
if(level>0)
{
UDEBUG("");
tmpMaxDisparity = tmpMinDisparity+(bestScoreIndex+1)*(1<<level);
tmpMaxDisparity+=tmpMaxDisparity%level;
if(tmpMaxDisparity > maxDisparity)
@@ -241,6 +247,7 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
if(bestScoreIndex>=0)
{
UDEBUG("");
//subpixel refining
int d = -(tmpMinDisparity+bestScoreIndex);
@@ -314,21 +321,20 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
}
}
if(leftCorners[i].x+float(d) == xc)
{
++noSubPixel;
}
rightCorners[i] = cv::Point2f(xc, leftCorners[i].y);
status[i] = reject?0:1;
if(!reject)
{
if(leftCorners[i].x+float(d) != xc)
{
++noSubPixel;
}
++added;
}
}
subpixelTime+=timer.ticks();
}
UDEBUG("noSubPixel=%d/%d", noSubPixel, added);
UDEBUG("SubPixel=%d/%d added (total=%d)", noSubPixel, added, (int)status.size());
UDEBUG("totalIterations=%d", totalIterations);
UDEBUG("Time pyramid = %f s", pyramidTime);
UDEBUG("Time disparity = %f s", disparityTime);
corelib/src/util3d_features.cpp
+2 -2
View File
@@ -183,7 +183,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
int pnpIterations,
float pnpReprojError,
int pnpFlags,
bool pnpOpenCV2,
int pnpRefineIterations,
float ransacParam1,
float ransacParam2,
const std::map<int, cv::Point3f> & refGuess3D,
@@ -401,7 +401,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
0, // min inliers
inliersV,
pnpFlags,
pnpOpenCV2);
pnpRefineIterations);
UDEBUG("PnP inliers = %d / %d", (int)inliersV.size(), (int)objectPoints.size());
corelib/src/util3d_motion_estimation.cpp
+152 -300
View File
@@ -49,7 +49,7 @@ Transform estimateMotion3DTo2D(
int iterations,
double reprojError,
int flagsPnP,
bool pnpOpenCV2,
int refineIterations,
const Transform & guess,
const std::map<int, cv::Point3f> & words3B,
double * varianceOut,
@@ -94,6 +94,7 @@ Transform estimateMotion3DTo2D(
{
//PnPRansac
cv::Mat K = cameraModel.K();
cv::Mat D = cameraModel.D();
Transform guessCameraFrame = (guess * cameraModel.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guessCameraFrame.r11(), (double)guessCameraFrame.r12(), (double)guessCameraFrame.r13(),
@@ -109,7 +110,7 @@ Transform estimateMotion3DTo2D(
objectPoints,
imagePoints,
K,
cv::Mat(),
D,
rvec,
tvec,
true,
@@ -118,7 +119,7 @@ Transform estimateMotion3DTo2D(
0, // min inliers
inliers,
flagsPnP,
pnpOpenCV2);
refineIterations);
if((int)inliers.size() >= minInliers)
{
@@ -234,9 +235,8 @@ Transform estimateMotion3DTo3D(
inliers1cloud,
inliersDistance,
iterations,
refineIterations>0,
3.0,
refineIterations,
3.0,
&inliers,
varianceOut);
@@ -262,317 +262,63 @@ Transform estimateMotion3DTo3D(
return transform;
}
// Don't know why, but the RANSAC implementation in OpenCV 3 gives me far
// more wrong results than the 2.4x implementation.
// Here is a copy of the RANSAC implementation from OpenCV 2.4.x version
#if CV_MAJOR_VERSION >= 3
namespace pnpransac
{
const int MIN_POINTS_COUNT = 4;
static void project3dPoints(const cv::Mat& points, const cv::Mat& rvec, const cv::Mat& tvec, cv::Mat& modif_points)
std::vector<float> computeReprojErrors(
std::vector<cv::Point3f> opoints,
std::vector<cv::Point2f> ipoints,
const cv::Mat & cameraMatrix,
const cv::Mat & distCoeffs,
const cv::Mat & rvec,
const cv::Mat & tvec,
float reprojErrorThreshold,
std::vector<int> & inliers)
{
modif_points.create(1, points.cols, CV_32FC3);
cv::Mat R(3, 3, CV_64FC1);
cv::Rodrigues(rvec, R);
cv::Mat transformation(3, 4, CV_64F);
cv::Mat r = transformation.colRange(0, 3);
R.copyTo(r);
cv::Mat t = transformation.colRange(3, 4);
tvec.copyTo(t);
transform(points, modif_points, transformation);
}
UASSERT(opoints.size() == ipoints.size());
int count = (int)opoints.size();
struct CameraParameters
{
void init(cv::Mat _intrinsics, cv::Mat _distCoeffs)
{
_intrinsics.copyTo(intrinsics);
_distCoeffs.copyTo(distortion);
}
std::vector<cv::Point2f> projpoints;
projectPoints(opoints, rvec, tvec, cameraMatrix, distCoeffs, projpoints);
cv::Mat intrinsics;
cv::Mat distortion;
};
struct Parameters
inliers.resize(count,0);
std::vector<float> err(count);
int oi=0;
for (int i = 0; i < count; ++i)
{
int iterationsCount;
float reprojectionError;
int minInliersCount;
bool useExtrinsicGuess;
int flags;
CameraParameters camera;
};
template <typename OpointType, typename IpointType>
static void pnpTask(const int curIndex, const std::vector<char>& pointsMask, const cv::Mat& objectPoints, const cv::Mat& imagePoints,
const Parameters& params, std::vector<int>& inliers, int& bestIndex, cv::Mat& rvec, cv::Mat& tvec,
const cv::Mat& rvecInit, const cv::Mat& tvecInit, cv::Mutex& resultsMutex)
float e = (float)cv::norm( ipoints[i] - projpoints[i]);
if(e <= reprojErrorThreshold)
{
cv::Mat modelObjectPoints(1, MIN_POINTS_COUNT, CV_MAKETYPE(cv::DataDepth<OpointType>::value, 3));
cv::Mat modelImagePoints(1, MIN_POINTS_COUNT, CV_MAKETYPE(cv::DataDepth<IpointType>::value, 2));
for (int i = 0, colIndex = 0; i < (int)pointsMask.size(); i++)
{
if (pointsMask[i])
{
cv::Mat colModelImagePoints = modelImagePoints(cv::Rect(colIndex, 0, 1, 1));
imagePoints.col(i).copyTo(colModelImagePoints);
cv::Mat colModelObjectPoints = modelObjectPoints(cv::Rect(colIndex, 0, 1, 1));
objectPoints.col(i).copyTo(colModelObjectPoints);
colIndex = colIndex+1;
inliers[oi] = i;
err[oi++] = e;
}
}
//filter same 3d points, hang in solvePnP
double eps = 1e-10;
int num_same_points = 0;
for (int i = 0; i < MIN_POINTS_COUNT; i++)
for (int j = i + 1; j < MIN_POINTS_COUNT; j++)
{
if (norm(modelObjectPoints.at<cv::Vec<OpointType,3> >(0, i) - modelObjectPoints.at<cv::Vec<OpointType,3> >(0, j)) < eps)
num_same_points++;
inliers.resize(oi);
err.resize(oi);
return err;
}
if (num_same_points > 0)
return;
cv::Mat localRvec, localTvec;
rvecInit.copyTo(localRvec);
tvecInit.copyTo(localTvec);
// OpenCV 3
cv::solvePnP(
modelObjectPoints,
modelImagePoints,
params.camera.intrinsics,
params.camera.distortion,
localRvec,
localTvec,
params.useExtrinsicGuess,
params.flags);
std::vector<cv::Point_<OpointType> > projected_points;
projected_points.resize(objectPoints.cols);
projectPoints(objectPoints, localRvec, localTvec, params.camera.intrinsics, params.camera.distortion, projected_points);
cv::Mat rotatedPoints;
project3dPoints(objectPoints, localRvec, localTvec, rotatedPoints);
std::vector<int> localInliers;
for (int i = 0; i < objectPoints.cols; i++)
{
//Although p is a 2D point it needs the same type as the object points to enable the norm calculation
cv::Point_<OpointType> p((OpointType)imagePoints.at<cv::Vec<IpointType,2> >(0, i)[0],
(OpointType)imagePoints.at<cv::Vec<IpointType,2> >(0, i)[1]);
if ((norm(p - projected_points[i]) < params.reprojectionError)
&& (rotatedPoints.at<cv::Vec<OpointType,3> >(0, i)[2] > 0)) //hack
{
localInliers.push_back(i);
}
}
resultsMutex.lock();
if ( (localInliers.size() > inliers.size()) || (localInliers.size() == inliers.size() && curIndex > bestIndex))
{
inliers.clear();
inliers.resize(localInliers.size());
memcpy(&inliers[0], &localInliers[0], sizeof(int) * localInliers.size());
localRvec.copyTo(rvec);
localTvec.copyTo(tvec);
bestIndex = curIndex;
}
resultsMutex.unlock();
}
static void pnpTask(const int curIndex, const std::vector<char>& pointsMask, const cv::Mat& objectPoints, const cv::Mat& imagePoints,
const Parameters& params, std::vector<int>& inliers, int& bestIndex, cv::Mat& rvec, cv::Mat& tvec,
const cv::Mat& rvecInit, const cv::Mat& tvecInit, cv::Mutex& resultsMutex)
{
CV_Assert(objectPoints.depth() == CV_64F || objectPoints.depth() == CV_32F);
CV_Assert(imagePoints.depth() == CV_64F || imagePoints.depth() == CV_32F);
const bool objectDoublePrecision = objectPoints.depth() == CV_64F;
const bool imageDoublePrecision = imagePoints.depth() == CV_64F;
if(objectDoublePrecision)
{
if(imageDoublePrecision)
pnpTask<double, double>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
else
pnpTask<double, float>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
}
else
{
if(imageDoublePrecision)
pnpTask<float, double>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
else
pnpTask<float, float>(curIndex, pointsMask, objectPoints, imagePoints, params, inliers, bestIndex, rvec, tvec, rvecInit, tvecInit, resultsMutex);
}
}
// TBB removed
class PnPSolver
{
public:
void operator()(int begin, int end) const
{
std::vector<char> pointsMask(objectPoints.cols, 0);
for( int i=begin; i!=end; ++i )
{
memset(&pointsMask[0], 0, objectPoints.cols );
memset(&pointsMask[0], 1, MIN_POINTS_COUNT );
generateVar(pointsMask, rng_base_seed + i);
pnpTask(i, pointsMask, objectPoints, imagePoints, parameters,
inliers, bestIndex, rvec, tvec, initRvec, initTvec, syncMutex);
if ((int)inliers.size() >= parameters.minInliersCount)
{
break;
}
}
}
PnPSolver(const cv::Mat& _objectPoints, const cv::Mat& _imagePoints, const Parameters& _parameters,
cv::Mat& _rvec, cv::Mat& _tvec, std::vector<int>& _inliers, int& _bestIndex, uint64 _rng_base_seed):
objectPoints(_objectPoints), imagePoints(_imagePoints), parameters(_parameters),
rvec(_rvec), tvec(_tvec), inliers(_inliers), bestIndex(_bestIndex), rng_base_seed(_rng_base_seed)
{
bestIndex = -1;
rvec.copyTo(initRvec);
tvec.copyTo(initTvec);
}
private:
PnPSolver& operator=(const PnPSolver&);
const cv::Mat& objectPoints;
const cv::Mat& imagePoints;
const Parameters& parameters;
cv::Mat &rvec, &tvec;
std::vector<int>& inliers;
int& bestIndex;
const uint64 rng_base_seed;
cv::Mat initRvec, initTvec;
static cv::Mutex syncMutex;
void generateVar(std::vector<char>& mask, uint64 rng_seed) const
{
cv::RNG generator(rng_seed);
int size = (int)mask.size();
for (int i = 0; i < size; i++)
{
int i1 = generator.uniform(0, size);
int i2 = generator.uniform(0, size);
char curr = mask[i1];
mask[i1] = mask[i2];
mask[i2] = curr;
}
}
};
cv::Mutex PnPSolver::syncMutex;
}
#endif
void solvePnPRansac(
cv::InputArray _opoints,
cv::InputArray _ipoints,
cv::InputArray _cameraMatrix,
cv::InputArray _distCoeffs,
cv::OutputArray _rvec,
cv::OutputArray _tvec,
const std::vector<cv::Point3f> & objectPoints,
const std::vector<cv::Point2f> & imagePoints,
const cv::Mat & cameraMatrix,
const cv::Mat & distCoeffs,
cv::Mat & rvec,
cv::Mat & tvec,
bool useExtrinsicGuess,
int iterationsCount,
float reprojectionError,
int minInliersCount,
cv::OutputArray _inliers,
std::vector<int> & inliers,
int flags,
bool opencv2version)
{
#if CV_MAJOR_VERSION >= 3
if(opencv2version)
{
const int _rng_seed = 0;
cv::Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
cv::Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
CV_Assert(opoints.isContinuous());
CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F);
CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
CV_Assert(ipoints.isContinuous());
CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
_rvec.create(3, 1, CV_64FC1);
_tvec.create(3, 1, CV_64FC1);
cv::Mat rvec = _rvec.getMat();
cv::Mat tvec = _tvec.getMat();
cv::Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);
if (minInliersCount <= 0)
minInliersCount = objectPoints.cols;
pnpransac::Parameters params;
params.iterationsCount = iterationsCount;
params.minInliersCount = minInliersCount;
params.reprojectionError = reprojectionError;
params.useExtrinsicGuess = useExtrinsicGuess;
params.camera.init(cameraMatrix, distCoeffs);
params.flags = flags;
std::vector<int> localInliers;
cv::Mat localRvec, localTvec;
rvec.copyTo(localRvec);
tvec.copyTo(localTvec);
int bestIndex;
// TBB not used
if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT)
{
pnpransac::PnPSolver solver(objectPoints, imagePoints, params,
localRvec, localTvec, localInliers, bestIndex,
_rng_seed);
solver(0, iterationsCount);
}
if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT)
{
if (flags != CV_P3P)
{
int i, pointsCount = (int)localInliers.size();
cv::Mat inlierObjectPoints(1, pointsCount, CV_MAKE_TYPE(opoints.depth(), 3)), inlierImagePoints(1, pointsCount, CV_MAKE_TYPE(ipoints.depth(), 2));
for (i = 0; i < pointsCount; i++)
{
int index = localInliers[i];
cv::Mat colInlierImagePoints = inlierImagePoints(cv::Rect(i, 0, 1, 1));
imagePoints.col(index).copyTo(colInlierImagePoints);
cv::Mat colInlierObjectPoints = inlierObjectPoints(cv::Rect(i, 0, 1, 1));
objectPoints.col(index).copyTo(colInlierObjectPoints);
}
solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, false, flags);
}
localRvec.copyTo(rvec);
localTvec.copyTo(tvec);
if (_inliers.needed())
cv::Mat(localInliers).copyTo(_inliers);
}
else
{
tvec.setTo(cv::Scalar(0));
cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
Rodrigues(R, rvec);
if( _inliers.needed() )
_inliers.release();
}
}
else
#endif
int refineIterations,
float refineSigma)
{
cv::solvePnPRansac(
_opoints,
_ipoints,
_cameraMatrix,
_distCoeffs,
_rvec,
_tvec,
objectPoints,
imagePoints,
cameraMatrix,
distCoeffs,
rvec,
tvec,
useExtrinsicGuess,
iterationsCount,
reprojectionError,
@@ -581,10 +327,116 @@ void solvePnPRansac(
#else
0.99, // confidence
#endif
_inliers,
inliers,
flags);
float inlierThreshold = reprojectionError;
if(inliers.size() >= 4 && refineIterations>0)
{
float inlier_distance_threshold_sqr = inlierThreshold * inlierThreshold;
float error_threshold = inlierThreshold;
float sigma_sqr = refineSigma * refineSigma;
int refine_iterations = 0;
bool inlier_changed = false, oscillating = false;
std::vector<int> new_inliers, prev_inliers = inliers;
std::vector<size_t> inliers_sizes;
//Eigen::VectorXf new_model_coefficients = model_coefficients;
cv::Mat new_model_rvec = rvec;
cv::Mat new_model_tvec = tvec;
do
{
// Get inliers from the current model
std::vector<cv::Point3f> opoints_inliers(prev_inliers.size());
std::vector<cv::Point2f> ipoints_inliers(prev_inliers.size());
for(unsigned int i=0; i<prev_inliers.size(); ++i)
{
opoints_inliers[i] = objectPoints[prev_inliers[i]];
ipoints_inliers[i] = imagePoints[prev_inliers[i]];
}
UDEBUG("inliers=%d refine_iterations=%d, rvec=%f,%f,%f tvec=%f,%f,%f", (int)prev_inliers.size(), refine_iterations,
*new_model_rvec.ptr<double>(0), *new_model_rvec.ptr<double>(1), *new_model_rvec.ptr<double>(2),
*new_model_tvec.ptr<double>(0), *new_model_tvec.ptr<double>(1), *new_model_tvec.ptr<double>(2));
// Optimize the model coefficients
cv::solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix, distCoeffs, new_model_rvec, new_model_tvec, true, flags);
inliers_sizes.push_back(prev_inliers.size());
UDEBUG("rvec=%f,%f,%f tvec=%f,%f,%f",
*new_model_rvec.ptr<double>(0), *new_model_rvec.ptr<double>(1), *new_model_rvec.ptr<double>(2),
*new_model_tvec.ptr<double>(0), *new_model_tvec.ptr<double>(1), *new_model_tvec.ptr<double>(2));
// Select the new inliers based on the optimized coefficients and new threshold
std::vector<float> err = computeReprojErrors(objectPoints, imagePoints, cameraMatrix, distCoeffs, new_model_rvec, new_model_tvec, error_threshold, new_inliers);
UDEBUG("RANSAC refineModel: Number of inliers found (before/after): %d/%d, with an error threshold of %f.",
(int)prev_inliers.size (), (int)new_inliers.size (), error_threshold);
if (new_inliers.size() < 4)
{
++refine_iterations;
if (refine_iterations >= refineIterations)
{
break;
}
continue;
}
// Estimate the variance and the new threshold
float m = uMean(err.data(), err.size());
float variance = uVariance(err.data(), err.size());
error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
UDEBUG ("RANSAC refineModel: New estimated error threshold: %f (variance=%f mean=%f) on iteration %d out of %d.",
error_threshold, variance, m, refine_iterations, refineIterations);
inlier_changed = false;
std::swap (prev_inliers, new_inliers);
// If the number of inliers changed, then we are still optimizing
if (new_inliers.size () != prev_inliers.size ())
{
// Check if the number of inliers is oscillating in between two values
if (inliers_sizes.size () >= 4)
{
if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
{
oscillating = true;
break;
}
}
inlier_changed = true;
continue;
}
// Check the values of the inlier set
for (size_t i = 0; i < prev_inliers.size (); ++i)
{
// If the value of the inliers changed, then we are still optimizing
if (prev_inliers[i] != new_inliers[i])
{
inlier_changed = true;
break;
}
}
}
while (inlier_changed && ++refine_iterations < refineIterations);
// If the new set of inliers is empty, we didn't do a good job refining
if (new_inliers.empty ())
{
UWARN ("RANSAC refineModel: Refinement failed: got an empty set of inliers!");
}
if (oscillating)
{
UDEBUG("RANSAC refineModel: Detected oscillations in the model refinement.");
}
std::swap (inliers, new_inliers);
rvec = new_model_rvec;
tvec = new_model_tvec;
}
return;
}
corelib/src/util3d_registration.cpp
+7 -8
View File
@@ -49,9 +49,8 @@ Transform transformFromXYZCorrespondences(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
double inlierThreshold,
int iterations,
bool refineModel,
double refineModelSigma,
int refineModelIterations,
int refineIterations,
double refineSigma,
std::vector<int> * inliersOut,
double * varianceOut)
{
@@ -97,11 +96,11 @@ Transform transformFromXYZCorrespondences(
sac.getInliers(inliers);
sac.getModelCoefficients (model_coefficients);
if (refineModel)
if (refineIterations>0)
{
double inlier_distance_threshold_sqr = inlierThreshold * inlierThreshold;
double error_threshold = inlierThreshold;
double sigma_sqr = refineModelSigma * refineModelSigma;
double sigma_sqr = refineSigma * refineSigma;
int refine_iterations = 0;
bool inlier_changed = false, oscillating = false;
std::vector<int> new_inliers, prev_inliers = inliers;
@@ -121,7 +120,7 @@ Transform transformFromXYZCorrespondences(
if (new_inliers.empty ())
{
++refine_iterations;
if (refine_iterations >= refineModelIterations)
if (refine_iterations >= refineIterations)
{
break;
}
@@ -133,7 +132,7 @@ Transform transformFromXYZCorrespondences(
error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
UDEBUG ("RANSAC refineModel: New estimated error threshold: %f (variance=%f) on iteration %d out of %d.",
error_threshold, variance, refine_iterations, refineModelIterations);
error_threshold, variance, refine_iterations, refineIterations);
inlier_changed = false;
std::swap (prev_inliers, new_inliers);
@@ -165,7 +164,7 @@ Transform transformFromXYZCorrespondences(
}
}
}
while (inlier_changed && ++refine_iterations < refineModelIterations);
while (inlier_changed && ++refine_iterations < refineIterations);
// If the new set of inliers is empty, we didn't do a good job refining
if (new_inliers.empty ())
guilib/include/rtabmap/gui/GraphViewer.h
+6
View File
@@ -57,6 +57,7 @@ public:
void updateGraph(const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
const std::map<int, int> & mapIds);
void updateGTGraph(const std::map<int, Transform> & poses);
void updateReferentialPosition(const Transform & t);
void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
void updatePosterior(const std::map<int, float> & posterior);
@@ -87,6 +88,7 @@ public:
const QColor & getRejectedLoopClosureColor() const {return _loopClosureRejectedColor;}
const QColor & getLocalPathColor() const {return _localPathColor;}
const QColor & getGlobalPathColor() const {return _globalPathColor;}
const QColor & getGTColor() const {return _gtPathColor;}
const QColor & getIntraSessionLoopColor() const {return _loopIntraSessionColor;}
const QColor & getInterSessionLoopColor() const {return _loopInterSessionColor;}
bool isIntraInterSessionColorsEnabled() const {return _intraInterSessionColors;}
@@ -112,6 +114,7 @@ public:
void setRejectedLoopClosureColor(const QColor & color);
void setLocalPathColor(const QColor & color);
void setGlobalPathColor(const QColor & color);
void setGTColor(const QColor & color);
void setIntraSessionLoopColor(const QColor & color);
void setInterSessionLoopColor(const QColor & color);
void setIntraInterSessionColorsEnabled(bool enabled);
@@ -145,12 +148,15 @@ private:
QColor _loopClosureRejectedColor;
QColor _localPathColor;
QColor _globalPathColor;
QColor _gtPathColor;
QColor _loopIntraSessionColor;
QColor _loopInterSessionColor;
bool _intraInterSessionColors;
QGraphicsItem * _root;
QMap<int, NodeItem*> _nodeItems;
QMultiMap<int, LinkItem*> _linkItems;
QMap<int, NodeItem*> _gtNodeItems;
QMultiMap<int, LinkItem*> _gtLinkItems;
QMultiMap<int, LinkItem*> _localPathLinkItems;
QMultiMap<int, LinkItem*> _globalPathLinkItems;
float _nodeRadius;
guilib/include/rtabmap/gui/MainWindow.h
+1
View File
@@ -292,6 +292,7 @@ private:
QMap<int, Signature> _cachedSignatures;
std::map<int, Transform> _currentPosesMap; // <nodeId, pose>
std::map<int, Transform> _currentGTPosesMap; // <nodeId, pose>
std::multimap<int, Link> _currentLinksMap; // <nodeFromId, link>
std::map<int, int> _currentMapIds; // <nodeId, mapId>
std::map<int, std::string> _curentLabels; // <nodeId, label>
guilib/src/GraphViewer.cpp
+151
View File
@@ -65,6 +65,7 @@ public:
this->setBrush(pen().color());
this->setAcceptHoverEvents(true);
}
virtual ~NodeItem() {}
void setColor(const QColor & color)
{
@@ -114,6 +115,7 @@ public:
{
this->setAcceptHoverEvents(true);
}
virtual ~LinkItem() {}
void setColor(const QColor & color)
{
@@ -183,6 +185,7 @@ GraphViewer::GraphViewer(QWidget * parent) :
_loopClosureRejectedColor(Qt::black),
_localPathColor(Qt::cyan),
_globalPathColor(Qt::darkMagenta),
_gtPathColor(Qt::gray),
_loopIntraSessionColor(Qt::red),
_loopInterSessionColor(Qt::green),
_intraInterSessionColors(false),
@@ -459,6 +462,124 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
UDEBUG("_nodeItems=%d, _linkItems=%d", _nodeItems.size(), _linkItems.size());
}
void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
{
bool wasEmpty = _gtNodeItems.size() == 0 && _gtLinkItems.size() == 0;
UDEBUG("poses=%d", (int)poses.size());
//Hide nodes and links
for(QMap<int, NodeItem*>::iterator iter = _gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
{
iter.value()->hide();
iter.value()->setColor(_gtPathColor); // reset color
}
for(QMultiMap<int, LinkItem*>::iterator iter = _gtLinkItems.begin(); iter!=_gtLinkItems.end(); ++iter)
{
iter.value()->hide();
}
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(!iter->second.isNull())
{
QMap<int, NodeItem*>::iterator itemIter = _gtNodeItems.find(iter->first);
if(itemIter != _gtNodeItems.end())
{
itemIter.value()->setPose(iter->second);
itemIter.value()->show();
}
else
{
// create node item
const Transform & pose = iter->second;
NodeItem * item = new NodeItem(iter->first, -1, pose, _nodeRadius);
this->scene()->addItem(item);
item->setZValue(20);
item->setColor(_gtPathColor);
item->setParentItem(_root);
_gtNodeItems.insert(iter->first, item);
}
if(iter!=poses.begin())
{
std::map<int, Transform>::const_iterator iterPrevious = iter;
--iterPrevious;
Transform previousPose = iterPrevious->second;
Transform currentPose = iter->second;
LinkItem * linkItem = 0;
QMultiMap<int, LinkItem*>::iterator linkIter = _gtLinkItems.end();
if(_gtLinkItems.contains(iterPrevious->first))
{
linkIter = _gtLinkItems.find(iter->first);
while(linkIter.key() == iterPrevious->first && linkIter != _gtLinkItems.end())
{
if(linkIter.value()->to() == iter->first)
{
linkIter.value()->setPoses(previousPose, currentPose);
linkIter.value()->show();
linkItem = linkIter.value();
break;
}
++linkIter;
}
}
if(linkItem == 0)
{
//create a link item
linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link::kUndef, 1);
QPen p = linkItem->pen();
p.setWidthF(_linkWidth);
linkItem->setPen(p);
linkItem->setZValue(10);
this->scene()->addItem(linkItem);
linkItem->setParentItem(_root);
_gtLinkItems.insert(iterPrevious->first, linkItem);
}
if(linkItem)
{
linkItem->setColor(_gtPathColor);
}
}
}
}
//remove not used nodes and links
for(QMap<int, NodeItem*>::iterator iter = _gtNodeItems.begin(); iter!=_gtNodeItems.end();)
{
if(!iter.value()->isVisible())
{
delete iter.value();
iter = _gtNodeItems.erase(iter);
}
else
{
++iter;
}
}
for(QMultiMap<int, LinkItem*>::iterator iter = _gtLinkItems.begin(); iter!=_gtLinkItems.end();)
{
if(!iter.value()->isVisible())
{
delete iter.value();
iter = _gtLinkItems.erase(iter);
}
else
{
++iter;
}
}
this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
if(wasEmpty)
{
QRectF rect = this->scene()->itemsBoundingRect();
this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
}
UDEBUG("_gtNodeItems=%d, _gtLinkItems=%d", _gtNodeItems.size(), _gtLinkItems.size());
}
void GraphViewer::updateReferentialPosition(const Transform & t)
{
QTransform qt(t.r11(), t.r12(), t.r21(), t.r22(), -t.o24(), -t.o14());
@@ -695,6 +816,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
settings.setValue("rejected_color", this->getRejectedLoopClosureColor());
settings.setValue("local_path_color", this->getLocalPathColor());
settings.setValue("global_path_color", this->getGlobalPathColor());
settings.setValue("gt_color", this->getGTColor());
settings.setValue("intra_session_color", this->getIntraSessionLoopColor());
settings.setValue("inter_session_color", this->getInterSessionLoopColor());
settings.setValue("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled());
@@ -729,6 +851,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
this->setRejectedLoopClosureColor(settings.value("rejected_color", this->getRejectedLoopClosureColor()).value<QColor>());
this->setLocalPathColor(settings.value("local_path_color", this->getLocalPathColor()).value<QColor>());
this->setGlobalPathColor(settings.value("global_path_color", this->getGlobalPathColor()).value<QColor>());
this->setGTColor(settings.value("gt_color", this->getGTColor()).value<QColor>());
this->setIntraSessionLoopColor(settings.value("intra_session_color", this->getIntraSessionLoopColor()).value<QColor>());
this->setInterSessionLoopColor(settings.value("inter_session_color", this->getInterSessionLoopColor()).value<QColor>());
this->setGridMapVisible(settings.value("grid_visible", this->isGridMapVisible()).toBool());
@@ -772,6 +895,10 @@ void GraphViewer::setNodeRadius(float radius)
{
iter.value()->setRect(-_nodeRadius, -_nodeRadius, _nodeRadius*2.0f, _nodeRadius*2.0f);
}
for(QMap<int, NodeItem*>::iterator iter=_gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
{
iter.value()->setRect(-_nodeRadius, -_nodeRadius, _nodeRadius*2.0f, _nodeRadius*2.0f);
}
}
void GraphViewer::setLinkWidth(float width)
{
@@ -887,6 +1014,18 @@ void GraphViewer::setGlobalPathColor(const QColor & color)
{
_globalPathColor = color;
}
void GraphViewer::setGTColor(const QColor & color)
{
_gtPathColor = color;
for(QMap<int, NodeItem*>::iterator iter=_gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
{
iter.value()->setColor(_gtPathColor);
}
for(QMultiMap<int, LinkItem*>::iterator iter=_gtLinkItems.begin(); iter!=_gtLinkItems.end(); ++iter)
{
iter.value()->setColor(_gtPathColor);
}
}
void GraphViewer::setIntraSessionLoopColor(const QColor & color)
{
_loopIntraSessionColor = color;
@@ -1022,6 +1161,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
QAction * aChangeRejectedLoopThr = menuLink->addAction(tr("Set outlier threshold..."));
QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path"));
QAction * aChangeGlobalPathColor = menuLink->addAction(tr("Global path"));
QAction * aChangeGTColor = menuLink->addAction(tr("Ground truth"));
menuLink->addSeparator();
QAction * aSetIntraInterSessionColors = menuLink->addAction(tr("Enable intra/inter-session colors"));
QAction * aChangeIntraSessionLoopColor = menuLink->addAction(tr("Intra-session loop closure"));
@@ -1035,6 +1175,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
aChangeRejectedLoopColor->setIcon(createIcon(_loopClosureRejectedColor));
aChangeLocalPathColor->setIcon(createIcon(_localPathColor));
aChangeGlobalPathColor->setIcon(createIcon(_globalPathColor));
aChangeGTColor->setIcon(createIcon(_gtPathColor));
aChangeIntraSessionLoopColor->setIcon(createIcon(_loopIntraSessionColor));
aChangeInterSessionLoopColor->setIcon(createIcon(_loopInterSessionColor));
aChangeNeighborColor->setIconVisibleInMenu(true);
@@ -1046,6 +1187,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
aChangeRejectedLoopColor->setIconVisibleInMenu(true);
aChangeLocalPathColor->setIconVisibleInMenu(true);
aChangeGlobalPathColor->setIconVisibleInMenu(true);
aChangeGTColor->setIconVisibleInMenu(true);
aChangeIntraSessionLoopColor->setIconVisibleInMenu(true);
aChangeInterSessionLoopColor->setIconVisibleInMenu(true);
aSetIntraInterSessionColors->setCheckable(true);
@@ -1205,6 +1347,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
r == aChangeRejectedLoopColor ||
r == aChangeLocalPathColor ||
r == aChangeGlobalPathColor ||
r == aChangeGTColor ||
r == aChangeIntraSessionLoopColor ||
r == aChangeInterSessionLoopColor)
{
@@ -1249,6 +1392,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
color = _globalPathColor;
}
else if(r == aChangeGTColor)
{
color = _gtPathColor;
}
else if(r == aChangeIntraSessionLoopColor)
{
color = _loopIntraSessionColor;
@@ -1305,6 +1452,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
this->setGlobalPathColor(color);
}
else if(r == aChangeGTColor)
{
this->setGTColor(color);
}
else if(r == aChangeIntraSessionLoopColor)
{
this->setIntraSessionLoopColor(color);
guilib/src/MainWindow.cpp
+21
View File
@@ -1099,6 +1099,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
if(uContains(stat.getSignatures(), stat.refImageId()))
{
refMapId = stat.getSignatures().at(stat.refImageId()).mapId();
if(!stat.getSignatures().at(stat.refImageId()).sensorData().groundTruth().isNull())
{
_currentGTPosesMap.insert(std::make_pair(stat.refImageId(), stat.getSignatures().at(stat.refImageId()).sensorData().groundTruth()));
}
}
int highestHypothesisId = static_cast<float>(uValue(stat.data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
int loopId = stat.loopClosureId()>0?stat.loopClosureId():stat.localLoopClosureId()>0?stat.localLoopClosureId():highestHypothesisId;
@@ -1633,6 +1637,20 @@ void MainWindow::updateMapCloud(
kter->second->push_back(pt);
}
//Ground truth graph?
for(std::map<int, Transform>::iterator iter=_currentGTPosesMap.begin(); iter!=_currentGTPosesMap.end(); ++iter)
{
int mapId = -100;
//edges
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
if(kter == graphs.end())
{
kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>))).first;
}
pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
kter->second->push_back(pt);
}
// add graphs
for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter=graphs.begin(); iter!=graphs.end(); ++iter)
{
@@ -1677,6 +1695,8 @@ void MainWindow::updateMapCloud(
{
_ui->graphicsView_graphView->updateReferentialPosition(currentPose);
}
_ui->graphicsView_graphView->updateGTGraph(_currentGTPosesMap);
}
cv::Mat map8U;
if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) && (_createdScans.size() || _preferencesDialog->isGridMapFrom3DCloud()))
@@ -4078,6 +4098,7 @@ void MainWindow::clearTheCache()
_ui->widget_cloudViewer->clearTrajectory();
_ui->widget_mapVisibility->clear();
_currentPosesMap.clear();
_currentGTPosesMap.clear();
_currentLinksMap.clear();
_currentMapIds.clear();
_curentLabels.clear();
guilib/src/PreferencesDialog.cpp
+5 -7
View File
@@ -208,11 +208,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable());
_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
#if CV_MAJOR_VERSION < 3
_ui->loopClosure_pnpOpenCV2->setVisible(false);
_ui->label_loopClosure_pnpOpenCV2->setVisible(false);
#endif
// Default Driver
connect(_ui->comboBox_sourceType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateRGBDCameraGroupBoxVisibility()));
@@ -649,7 +644,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_bowEpipolarGeometryVar->setObjectName(Parameters::kVisEpipolarGeometryVar().c_str());
_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str());
_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());
_ui->loopClosure_pnpOpenCV2->setObjectName(Parameters::kVisPnPOpenCV2().c_str());
_ui->loopClosure_pnpRefineIterations->setObjectName(Parameters::kVisPnPRefineIterations().c_str());
_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kRegVarianceFromInliersCount().c_str());
_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
@@ -683,6 +678,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
//Odometry
_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
connect(_ui->odom_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_odometryType, SLOT(setCurrentIndex(int)));
_ui->odom_strategy->setCurrentIndex(Parameters::defaultOdomStrategy());
_ui->odom_countdown->setObjectName(Parameters::kOdomResetCountdown().c_str());
_ui->odom_holonomic->setObjectName(Parameters::kOdomHolonomic().c_str());
_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
@@ -2522,7 +2519,7 @@ void PreferencesDialog::selectSourceRGBDImagesPathGt()
{
list.push_back(_ui->comboBox_cameraRGBDImages_gtFormat->itemText(i));
}
QString item = QInputDialog::getItem(this, tr("Ground Truth Format"), tr("Format:"), list);
QString item = QInputDialog::getItem(this, tr("Ground Truth Format"), tr("Format:"), list, 0, false);
if(!item.isEmpty())
{
_ui->lineEdit_cameraRGBDImages_gt->setText(path);
@@ -3945,6 +3942,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
dir = d.absolutePath();
}
}
if(!camera->init(useRawImages?"":dir.toStdString(), name.toStdString()))
{
UWARN("init camera failed... ");
guilib/src/ui/preferencesDialog.ui
+394 -402
View File
@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<y>-366</y>
<width>633</width>
<height>2118</height>
</rect>
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>17</number>
<number>13</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -6430,7 +6430,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
</layout>
</widget>
<widget class="QWidget" name="page_23">
<layout class="QVBoxLayout" name="verticalLayout_2" stretch="0,1">
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QGroupBox" name="groupBox_bayes1">
<property name="title">
@@ -6579,26 +6579,6 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer_23">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<widget class="QWidget" name="page_19">
<layout class="QVBoxLayout" name="verticalLayout_13">
<item>
<widget class="QGroupBox" name="groupBox_vh_strategy2">
<property name="minimumSize">
@@ -6738,15 +6718,18 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
</layout>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer_3">
<spacer name="verticalSpacer_23">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>0</width>
<height>0</height>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
@@ -6799,7 +6782,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
<item row="0" column="1">
<widget class="QLabel" name="label_103">
<property name="text">
<string>Odometry strategy. More info corresponding panels.</string>
<string>Odometry strategy.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -6935,6 +6918,365 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
</item>
</layout>
</item>
<item>
<widget class="QStackedWidget" name="stackedWidget_odometryType">
<property name="currentIndex">
<number>0</number>
</property>
<widget class="QWidget" name="page_52">
<layout class="QVBoxLayout" name="verticalLayout_77">
<item>
<widget class="QGroupBox" name="groupBox_odometryBOW2">
<property name="title">
<string>Features Map</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_54">
<item>
<widget class="QLabel" name="label_226">
<property name="text">
<string>Features extracted from frames are matached a using nearest neighbor approach. It maintains a local map of features to match to.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_29" columnstretch="0,0,1">
<item row="0" column="2">
<widget class="QLabel" name="label_190">
<property name="text">
<string>Local history size: If &gt; 0 (example 5000), the odometry will maintain a local map of X maximum words. This will decrease odometry drifting when the camera is not moving.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLineEdit" name="odom_fixedLocalMapPath"/>
</item>
<item row="0" column="0">
<widget class="QSpinBox" name="odom_localHistory">
<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>0</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QToolButton" name="toolButton_odomBowFixedLocalMap">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_239">
<property name="text">
<string>Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP pose estimation is activated.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer_46">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>0</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="page_58">
<layout class="QVBoxLayout" name="verticalLayout_55">
<item>
<widget class="QGroupBox" name="groupBox_odometryFlow2">
<property name="title">
<string>Frame to Frame</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_53">
<item>
<widget class="QLabel" name="label_199">
<property name="text">
<string>Features from the last frame are estimated in the new frame (see Visual Registration parameters).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_37" columnstretch="0,1">
<item row="0" column="1">
<widget class="QLabel" name="label_196">
<property name="text">
<string>Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</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="QSpinBox" name="odom_flow_keyframeThr">
<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>0</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_198">
<property name="text">
<string>Guess optical flow based on previous motion.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="odom_flow_guessMotion">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer_43">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>0</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="page_26">
<layout class="QVBoxLayout" name="verticalLayout_88">
<item>
<widget class="QGroupBox" name="groupBox_odometryMono2">
<property name="title">
<string>Mono</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_168">
<item>
<widget class="QLabel" name="label_672">
<property name="text">
<string>Mono is for single camera motion estimation (MonoSLAM). On initialization, the camera must be translated on the side until a first transform can be computed.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_661">
<property name="text">
<string>Parameters from BOW and OpticalFlow are also used here. PnP parameters on Odometry panel are used too.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_166" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_minFlow">
<property name="suffix">
<string> pixels</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="maximum">
<double>1000.000000000000000</double>
</property>
<property name="singleStep">
<double>10.000000000000000</double>
</property>
<property name="value">
<double>100.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_659">
<property name="text">
<string>Minimum optical flow required for the initialization step.</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="doubleSpinBox_minTranslation">
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.020000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_660">
<property name="text">
<string>Minimum translation to add new points to local map. On initialization, translation x 5 is used as the required minimum transformation.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_662">
<property name="text">
<string>Minimum translation required for the initialization step.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_minInitTranslation">
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_663">
<property name="text">
<string>Maximum variance to add new points to local map.</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="doubleSpinBox_maxVariance">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="singleStep">
<double>0.001000000000000</double>
</property>
<property name="value">
<double>0.010000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
</layout>
</widget>
</widget>
</item>
<item>
<widget class="QStackedWidget" name="stackedWidget_odometryFiltering">
<property name="currentIndex">
@@ -7282,371 +7624,6 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
</item>
</layout>
</widget>
<widget class="QWidget" name="page_26">
<layout class="QVBoxLayout" name="verticalLayout_77">
<item>
<widget class="QGroupBox" name="groupBox_odometryBOW2">
<property name="title">
<string>Features Map</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_54">
<item>
<widget class="QLabel" name="label_226">
<property name="text">
<string>Features extracted from frames are matached a using nearest neighbor approach. It maintains a local map of features to match to.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_29" columnstretch="0,0,1">
<item row="0" column="2">
<widget class="QLabel" name="label_190">
<property name="text">
<string>Local history size: If &gt; 0 (example 5000), the odometry will maintain a local map of X maximum words. This will decrease odometry drifting when the camera is not moving.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLineEdit" name="odom_fixedLocalMapPath"/>
</item>
<item row="0" column="0">
<widget class="QSpinBox" name="odom_localHistory">
<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>0</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QToolButton" name="toolButton_odomBowFixedLocalMap">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_239">
<property name="text">
<string>Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP pose estimation is activated.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer_27">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>670</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<widget class="QWidget" name="page_28">
<layout class="QVBoxLayout" name="verticalLayout_55">
<item>
<widget class="QGroupBox" name="groupBox_odometryFlow2">
<property name="title">
<string>Frame to Frame</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_53">
<item>
<widget class="QLabel" name="label_199">
<property name="text">
<string>Features from the last frame are estimated in the new frame (see Visual Registration parameters).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_37" columnstretch="0,1">
<item row="0" column="1">
<widget class="QLabel" name="label_196">
<property name="text">
<string>Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</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="QSpinBox" name="odom_flow_keyframeThr">
<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>0</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_198">
<property name="text">
<string>Guess optical flow based on previous motion.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="odom_flow_guessMotion">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer_28">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>518</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<widget class="QWidget" name="page_30">
<layout class="QVBoxLayout" name="verticalLayout_169">
<item>
<widget class="QGroupBox" name="groupBox_odometryMono2">
<property name="title">
<string>Mono</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_168">
<item>
<widget class="QLabel" name="label_672">
<property name="text">
<string>Mono is for single camera motion estimation (MonoSLAM). On initialization, the camera must be translated on the side until a first transform can be computed.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_661">
<property name="text">
<string>Parameters from BOW and OpticalFlow are also used here. PnP parameters on Odometry panel are used too.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_166" columnstretch="0,1">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_minFlow">
<property name="suffix">
<string> pixels</string>
</property>
<property name="decimals">
<number>0</number>
</property>
<property name="maximum">
<double>1000.000000000000000</double>
</property>
<property name="singleStep">
<double>10.000000000000000</double>
</property>
<property name="value">
<double>100.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_659">
<property name="text">
<string>Minimum optical flow required for the initialization step.</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="doubleSpinBox_minTranslation">
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.020000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_660">
<property name="text">
<string>Minimum translation to add new points to local map. On initialization, translation x 5 is used as the required minimum transformation.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_662">
<property name="text">
<string>Minimum translation required for the initialization step.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_minInitTranslation">
<property name="suffix">
<string> m</string>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_663">
<property name="text">
<string>Maximum variance to add new points to local map.</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="doubleSpinBox_maxVariance">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="singleStep">
<double>0.001000000000000</double>
</property>
<property name="value">
<double>0.010000000000000</double>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer_30">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<widget class="QWidget" name="page_46">
<layout class="QVBoxLayout" name="verticalLayout_81">
<item>
@@ -7844,7 +7821,7 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
<item row="7" column="1">
<widget class="QLabel" name="label_298">
<property name="text">
<string>Forward estimation (A-&gt;B). If false, a transformation is also computed in backward direction (B-&gt;A), then the two resulting transforms are merged (middle interpolation between the transforms).</string>
<string>Forward estimation only (A-&gt;B). If false, a transformation is also computed in backward direction (B-&gt;A), then the two resulting transforms are merged (middle interpolation between the transforms).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -7866,10 +7843,13 @@ see Sqlite3 doc 'PRAGMA temp_store'.</string>
<item>
<widget class="QStackedWidget" name="stackedWidget_loopClosureCorrespondences">
<property name="currentIndex">
<number>0</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_54">
<layout class="QVBoxLayout" name="verticalLayout_85">
<property name="margin">
<number>0</number>
</property>
<item>
<widget class="QGroupBox" name="groupBox_4">
<property name="title">
@@ -7978,6 +7958,9 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</widget>
<widget class="QWidget" name="page_55">
<layout class="QVBoxLayout" name="verticalLayout_86">
<property name="margin">
<number>0</number>
</property>
<item>
<widget class="QGroupBox" name="groupBox_7">
<property name="title">
@@ -8286,17 +8269,10 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QCheckBox" name="loopClosure_pnpOpenCV2">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_loopClosure_pnpOpenCV2">
<property name="text">
<string>Use OpenCV2 solvePnPRansac() in OpenCV3.</string>
<string>Refine iterations.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -8306,6 +8282,22 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</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>
</layout>
</widget>
</item>