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

Refactored OdometryOpticalFlow (added optical flow guess using previous odometry transform, merged stereo/depth stuff)

Browse Source
This commit is contained in:
matlabbe
2015-06-15 14:44:44 -04:00
parent b8dccc2228
commit dfbf6e721e
14 changed files with 785 additions and 671 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
corelib/include/rtabmap/core/Odometry.h
View File

@@ -63,6 +63,7 @@ public:
bool isPnPEstimationUsed() const {return _pnpEstimation;}
double getPnPReprojError() const {return _pnpReprojError;}
int getPnPFlags() const {return _pnpFlags;}
const Transform & previousTransform() const {return previousTransform_;}
private:
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
@@ -89,6 +90,8 @@ private:
Transform _pose;
int _resetCurrentCount;
double previousStamp_;
Transform previousTransform_;
float distanceTravelled_;
std::vector<ParticleFilter *> filters_;
@@ -133,9 +136,7 @@ public:
private:
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);
Transform computeTransformStereo(const SensorData & image, OdometryInfo * info);
Transform computeTransformRGBD(const SensorData & image, OdometryInfo * info);
Transform computeTransformMono(const SensorData & image, OdometryInfo * info);
private:
//Parameters:
int flowWinSize_;
@@ -156,7 +157,6 @@ private:
Feature2D * feature2D_;
cv::Mat refFrame_;
cv::Mat refRightFrame_;
std::vector<cv::Point2f> refCorners_;
pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_;
};

2
corelib/include/rtabmap/core/OdometryInfo.h
View File

@@ -43,6 +43,7 @@ public:
timeEstimation(-1),
stamp(0),
interval(0),
distanceTravelled(0),
type(-1)
{}
bool lost;
@@ -57,6 +58,7 @@ public:
double interval;
Transform transform;
Transform transformFiltered;
float distanceTravelled;
int type; // 0=BOW, 1=Optical Flow, 2=ICP

17
corelib/include/rtabmap/core/util3d_features.h
View File

@@ -73,7 +73,22 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
int flowWinSize = 9,
int flowMaxLevel = 4,
int flowIterations = 20,
double flowEps = 0.02);
double flowEps = 0.02,
double maxCorrespondencesSlope = 0.0);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
const std::vector<cv::Point2f> & leftCorners,
const cv::Mat & leftImage,
const cv::Mat & rightImage,
float fx,
float baseline,
float cx,
float cy,
const Transform & transform = Transform::getIdentity(),
int flowWinSize = 9,
int flowMaxLevel = 4,
int flowIterations = 20,
double flowEps = 0.02,
double maxCorrespondencesSlope = 0.0);
std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3DMono(
const std::multimap<int, cv::KeyPoint> & kpts,

48
corelib/src/Odometry.cpp
View File

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
#include "ParticleFilter.h"
namespace rtabmap {
@@ -53,7 +54,9 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_pnpReprojError(Parameters::defaultOdomPnPReprojError()),
_pnpFlags(Parameters::defaultOdomPnPFlags()),
_resetCurrentCount(0),
previousStamp_(0)
previousStamp_(0),
previousTransform_(Transform::getIdentity()),
distanceTravelled_(0)
{
Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers);
@@ -106,8 +109,10 @@ Odometry::~Odometry()
void Odometry::reset(const Transform & initialPose)
{
previousTransform_.setIdentity();
_resetCurrentCount = 0;
previousStamp_ = 0;
distanceTravelled_ = 0;
if(_force2D || filters_.size())
{
float x,y,z, roll,pitch,yaw;
@@ -178,6 +183,8 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
info->interval = data.stamp() - previousStamp_;
info->transform = t;
}
previousTransform_.setIdentity();
previousStamp_ = data.stamp();
if(!t.isNull())
@@ -192,15 +199,27 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
if(filters_.size())
{
UASSERT(filters_.size()==6);
x = filters_[0]->filter(x);
y = filters_[1]->filter(y);
yaw = filters_[5]->filter(yaw);
if(!_force2D)
if(_pose.isIdentity())
{
z = filters_[2]->filter(z);
roll = filters_[3]->filter(roll);
pitch = filters_[4]->filter(pitch);
filters_[0]->init(x);
filters_[1]->init(y);
filters_[2]->init(z);
filters_[3]->init(roll);
filters_[4]->init(pitch);
filters_[5]->init(yaw);
}
else
{
x = filters_[0]->filter(x);
y = filters_[1]->filter(y);
yaw = filters_[5]->filter(yaw);
if(!_force2D)
{
z = filters_[2]->filter(z);
roll = filters_[3]->filter(roll);
pitch = filters_[4]->filter(pitch);
}
}
if(info)
@@ -208,6 +227,10 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
info->timeParticleFiltering = time.ticks();
}
}
UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) &&
uIsFinite(roll) && uIsFinite(pitch) && uIsFinite(yaw),
uFormat("x=%f y=%f z=%f roll=%f pitch=%f yaw=%f org T=%s",
x, y, z, roll, pitch, yaw, t.prettyPrint().c_str()).c_str());
t = Transform(x,y,_force2D?0:z, _force2D?0:roll,_force2D?0:pitch,yaw);
if(info)
@@ -216,6 +239,13 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
}
}
previousTransform_ = t;
if(info)
{
distanceTravelled_ += t.getNorm();
info->distanceTravelled = distanceTravelled_;
}
return _pose *= t; // updated
}
else if(_resetCurrentCount > 0)

807
corelib/src/OdometryOpticalFlow.cpp
View File

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_features.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
@@ -120,29 +121,6 @@ void OdometryOpticalFlow::reset(const Transform & initialPose)
Transform OdometryOpticalFlow::computeTransform(
const SensorData & data,
OdometryInfo * info)
{
UDEBUG("");
if(info)
{
info->type = 1;
}
if(!data.rightImage().empty())
{
//stereo
return computeTransformStereo(data, info);
}
else
{
//rgbd
return computeTransformRGBD(data, info);
}
}
Transform OdometryOpticalFlow::computeTransformStereo(
const SensorData & data,
OdometryInfo * info)
{
UTimer timer;
Transform output;
@@ -151,6 +129,11 @@ Transform OdometryOpticalFlow::computeTransformStereo(
int inliers = 0;
int correspondences = 0;
if(info)
{
info->type = 1;
}
cv::Mat newLeftFrame;
// convert to grayscale
if(data.image().channels() > 1)
@@ -161,17 +144,50 @@ Transform OdometryOpticalFlow::computeTransformStereo(
{
newLeftFrame = data.image().clone();
}
cv::Mat newRightFrame = data.rightImage().clone();
std::vector<cv::Point2f> newCorners;
UDEBUG("lastCorners_.size()=%d lastFrame_=%d lastRightFrame_=%d", (int)refCorners_.size(), refFrame_.empty()?0:1, refRightFrame_.empty()?0:1);
if(!refFrame_.empty() && !refRightFrame_.empty() && refCorners_.size())
UDEBUG("lastCorners_.size()=%d lastFrame_=%d depthRight=%d",
(int)refCorners_.size(), refFrame_.empty()?0:1, data.depthOrRightImage().empty()?0:1);
if(!refFrame_.empty() &&
!data.depthOrRightImage().empty() &&
refCorners_.size() &&
refCorners3D_->size())
{
UDEBUG("");
UASSERT_MSG(refCorners_.size() == refCorners3D_->size(),
uFormat("%d vs %d", (int)refCorners_.size(), (int)refCorners3D_->size()).c_str());
// make guess
bool flowGuessByMotion = true;
cv::Mat K = (cv::Mat_<double>(3,3) <<
data.fx(), 0, data.cx(),
0, data.fx(), data.cy(),
0, 0, 1);
Transform guess = (this->previousTransform() * data.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
std::vector<cv::Point3f> objectPoints(refCorners3D_->size());
for(unsigned int i=0; i<objectPoints.size(); ++i)
{
objectPoints[i].x = refCorners3D_->at(i).x;
objectPoints[i].y = refCorners3D_->at(i).y;
objectPoints[i].z = refCorners3D_->at(i).z;
}
if(flowGuessByMotion && !this->previousTransform().isIdentity())
{
UDEBUG("project points to new image");
cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), newCorners);
}
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
int winSize = (newCorners.size()||!flowGuessByMotion)?flowWinSize_:(flowWinSize_*2);
cv::calcOpticalFlowPyrLK(
refFrame_,
newLeftFrame,
@@ -179,155 +195,54 @@ Transform OdometryOpticalFlow::computeTransformStereo(
newCorners,
status,
err,
cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
cv::Size(winSize, winSize),
(newCorners.size()||!flowGuessByMotion)?flowMaxLevel_:flowMaxLevel_*2,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (newCorners.size()?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
std::vector<cv::Point2f> lastCornersKept(status.size());
pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3DKept(new pcl::PointCloud<pcl::PointXYZ>);
refCorners3DKept->resize(status.size());
std::vector<cv::Point3f> objectPointsKept(status.size());
std::vector<cv::Point2f> refCornersKept(status.size());
std::vector<cv::Point2f> newCornersKept(status.size());
int ki = 0;
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i])
{
lastCornersKept[ki] = refCorners_[i];
refCorners3DKept->at(ki) = refCorners3D_->at(i);
objectPointsKept[ki] = objectPoints[i];
refCornersKept[ki] = refCorners_[i];
newCornersKept[ki] = newCorners[i];
++ki;
}
}
lastCornersKept.resize(ki);
refCorners3DKept->resize(ki);
objectPointsKept.resize(ki);
refCornersKept.resize(ki);
newCornersKept.resize(ki);
if(ki && ki >= this->getMinInliers())
{
std::vector<unsigned char> statusLast;
std::vector<float> errLast;
std::vector<cv::Point2f> lastCornersKeptRight;
UDEBUG("previous stereo disparity");
cv::calcOpticalFlowPyrLK(
refFrame_,
refRightFrame_,
lastCornersKept,
lastCornersKeptRight,
statusLast,
errLast,
cv::Size(stereoWinSize_, stereoWinSize_), stereoMaxLevel_,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, stereoIterations_, stereoEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("new stereo disparity");
std::vector<unsigned char> statusNew;
std::vector<float> errNew;
std::vector<cv::Point2f> newCornersKeptRight;
cv::calcOpticalFlowPyrLK(
newLeftFrame,
newRightFrame,
newCornersKept,
newCornersKeptRight,
statusNew,
errNew,
cv::Size(stereoWinSize_, stereoWinSize_), stereoMaxLevel_,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, stereoIterations_, stereoEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
if(this->isPnPEstimationUsed())
{
// find correspondences
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(statusLast.size());
info->newCorners.resize(statusLast.size());
info->refCorners = refCornersKept;
info->newCorners = newCornersKept;
}
int flowInliers = 0;
std::vector<cv::Point3f> objectPoints(statusLast.size());
std::vector<cv::Point2f> imagePoints(statusLast.size());
std::vector<pcl::PointXYZ> image3DPoints(statusLast.size());
int oi=0;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(unsigned int i=0; i<statusLast.size(); ++i)
{
if(statusLast[i])
{
float lastDisparity = lastCornersKept[i].x - lastCornersKeptRight[i].x;
float lastSlope = fabs((lastCornersKept[i].y-lastCornersKeptRight[i].y) / (lastCornersKept[i].x-lastCornersKeptRight[i].x));
float newDisparity = newCornersKept[i].x - newCornersKeptRight[i].x;
float newSlope = fabs((newCornersKept[i].y-newCornersKeptRight[i].y) / (newCornersKept[i].x-newCornersKeptRight[i].x));
if(lastDisparity > 0.0f && lastSlope < stereoMaxSlope_)
{
pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D(
lastCornersKept[i],
lastDisparity,
data.cx(), data.cy(), data.fx(), data.baseline());
if(pcl::isFinite(lastPt3D) &&
(this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth())))
{
//Add 3D correspondences!
lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
objectPoints[oi].x = lastPt3D.x;
objectPoints[oi].y = lastPt3D.y;
objectPoints[oi].z = lastPt3D.z;
imagePoints[oi] = newCornersKept.at(i);
// new 3D points, used to compute variance
image3DPoints[oi] = pcl::PointXYZ(bad_point, bad_point, bad_point);
if(newDisparity > 0.0f && newSlope < stereoMaxSlope_)
{
pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D(
newCornersKept[i],
newDisparity,
data.cx(), data.cy(), data.fx(), data.baseline());
if(pcl::isFinite(newPt3D) &&
(this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth())))
{
image3DPoints[oi] = util3d::transformPoint(newPt3D, data.localTransform());
}
}
if(this->isInfoDataFilled() && info)
{
info->refCorners[oi] = lastCornersKept[i];
info->newCorners[oi] = newCornersKept[i];
}
++oi;
}
}
++flowInliers;
}
}
objectPoints.resize(oi);
imagePoints.resize(oi);
image3DPoints.resize(oi);
UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(oi);
info->newCorners.resize(oi);
}
correspondences = oi;
correspondences = refCornersKept.size();
if(correspondences >= this->getMinInliers())
{
//PnPRansac
cv::Mat K = (cv::Mat_<double>(3,3) <<
data.fx(), 0, data.cx(),
0, data.fx(), data.cy(),
0, 0, 1);
Transform guess = (data.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
std::vector<int> inliersV;
cv::solvePnPRansac(objectPoints,
imagePoints,
cv::solvePnPRansac(
objectPointsKept,
newCornersKept,
K,
cv::Mat(),
rvec,
@@ -339,39 +254,17 @@ Transform OdometryOpticalFlow::computeTransformStereo(
inliersV,
this->getPnPFlags());
cv::Rodrigues(rvec, R);
Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
inliers = (int)inliersV.size();
if((int)inliersV.size() >= this->getMinInliers())
{
cv::Rodrigues(rvec, R);
Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
// make it incremental
output = (data.localTransform() * pnp).inverse();
UDEBUG("Odom transform = %s", output.prettyPrint().c_str());
// compute variance (like in PCL computeVariance() method of sac_model.h)
std::vector<float> errorSqrdDists(inliersV.size());
int ii=0;
for(unsigned int i=0; i<inliersV.size(); ++i)
{
pcl::PointXYZ & newPt = image3DPoints[inliersV[i]];
if(pcl::isFinite(newPt))
{
newPt = util3d::transformPoint(newPt, output);
const cv::Point3f & objPt = objectPoints[inliersV[i]];
errorSqrdDists[ii++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
}
}
errorSqrdDists.resize(ii);
if(errorSqrdDists.size())
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
variance = 2.1981 * median_error_sqr;
}
variance = 1; // FIXME, is there a way to compute a variance from the PNP approach?
}
else
{
@@ -390,57 +283,79 @@ Transform OdometryOpticalFlow::computeTransformStereo(
}
else
{
UDEBUG("Getting correspondences begin");
// Get 3D correspondences
pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesRef(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
correspondencesLast->resize(statusLast.size());
correspondencesNew->resize(statusLast.size());
int oi = 0;
correspondencesRef->resize(newCornersKept.size());
correspondencesNew->resize(newCornersKept.size());
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(statusLast.size());
info->newCorners.resize(statusLast.size());
info->refCorners.resize(newCornersKept.size());
info->newCorners.resize(newCornersKept.size());
}
for(unsigned int i=0; i<statusLast.size(); ++i)
int oi = 0;
if(!data.rightImage().empty())
{
if(statusLast[i] && statusNew[i])
{
float lastDisparity = lastCornersKept[i].x - lastCornersKeptRight[i].x;
float newDisparity = newCornersKept[i].x - newCornersKeptRight[i].x;
float lastSlope = fabs((lastCornersKept[i].y-lastCornersKeptRight[i].y) / (lastCornersKept[i].x-lastCornersKeptRight[i].x));
float newSlope = fabs((newCornersKept[i].y-newCornersKeptRight[i].y) / (newCornersKept[i].x-newCornersKeptRight[i].x));
if(lastDisparity > 0.0f && newDisparity > 0.0f &&
lastSlope < stereoMaxSlope_ && newSlope < stereoMaxSlope_)
{
pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D(
lastCornersKept[i],
lastDisparity,
data.cx(), data.cy(), data.fx(), data.baseline());
pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D(
newCornersKept[i],
newDisparity,
data.cx(), data.cy(), data.fx(), data.baseline());
// stereo
pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D = util3d::generateKeypoints3DStereo(
newCornersKept,
newLeftFrame,
data.rightImage(),
data.fx(),
data.baseline(),
data.cx(),
data.cy(),
Transform::getIdentity(),
stereoWinSize_,
stereoMaxLevel_,
stereoIterations_,
stereoEps_,
stereoMaxSlope_);
if(pcl::isFinite(lastPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth())) &&
pcl::isFinite(newPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth())))
UASSERT(newCorners3D->size() == refCorners3DKept->size());
for(unsigned int i=0; i<newCorners3D->size(); ++i)
{
if(pcl::isFinite(newCorners3D->at(i)) && (this->getMaxDepth() <= 0.0f || newCorners3D->at(i).z < this->getMaxDepth()))
{
//Add 3D correspondences!
correspondencesRef->at(oi) = refCorners3DKept->at(i);
correspondencesNew->at(oi) = util3d::transformPoint(newCorners3D->at(i), data.localTransform());
if(this->isInfoDataFilled() && info)
{
info->refCorners[oi] = refCornersKept[i];
info->newCorners[oi] = newCornersKept[i];
}
++oi;
}
}// end loop
}
else
{
//depth
for(unsigned int i=0; i<newCornersKept.size(); ++i)
{
if(uIsInBounds(newCornersKept[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCornersKept[i].y, 0.0f, float(data.depth().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCornersKept[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true);
if(pcl::isFinite(pt) &&
(this->getMaxDepth() == 0.0f || pt.z < this->getMaxDepth()))
{
//Add 3D correspondences!
lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
newPt3D = util3d::transformPoint(newPt3D, data.localTransform());
correspondencesLast->at(oi) = lastPt3D;
correspondencesNew->at(oi) = newPt3D;
correspondencesRef->at(oi) = refCorners3DKept->at(i);
correspondencesNew->at(oi) = util3d::transformPoint(pt, data.localTransform());
if(this->isInfoDataFilled() && info)
{
info->refCorners[oi] = lastCornersKept[i];
info->refCorners[oi] = refCornersKept[i];
info->newCorners[oi] = newCornersKept[i];
}
++oi;
}
}
}
}// end loop
correspondencesLast->resize(oi);
}
correspondencesRef->resize(oi);
correspondencesNew->resize(oi);
if(this->isInfoDataFilled() && info)
{
@@ -448,8 +363,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
info->newCorners.resize(oi);
}
correspondences = oi;
refCorners3D_ = correspondencesNew;
UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)statusLast.size());
UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)newCornersKept.size());
if(correspondences >= this->getMinInliers())
{
@@ -457,7 +371,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
UTimer timerRANSAC;
Transform t = util3d::transformFromXYZCorrespondences(
correspondencesNew,
correspondencesLast,
correspondencesRef,
this->getInlierDistance(),
this->getIterations(),
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
@@ -499,11 +413,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
// Copy or generate new keypoints
if(data.keypoints().size())
{
newCorners.resize(data.keypoints().size());
for(unsigned int i=0; i<data.keypoints().size(); ++i)
{
newCorners[i] = data.keypoints().at(i).pt;
}
cv::KeyPoint::convert(data.keypoints(), newCorners);
}
else
{
@@ -528,11 +438,76 @@ Transform OdometryOpticalFlow::computeTransformStereo(
}
}
if((int)newCorners.size() > this->getMinInliers())
if((int)newCorners.size() >= this->getMinInliers())
{
refFrame_ = newLeftFrame;
refRightFrame_ = newRightFrame;
refCorners_ = newCorners;
pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
newCorners3D->resize(newCorners.size());
std::vector<cv::Point2f> newCornersFiltered(newCorners.size());
int oi=0;
if(!data.rightImage().empty())
{
/// stereo
pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3DTmp = util3d::generateKeypoints3DStereo(
newCorners,
newLeftFrame,
data.rightImage(),
data.fx(),
data.baseline(),
data.cx(),
data.cy(),
Transform::getIdentity(),
stereoWinSize_,
stereoMaxLevel_,
stereoIterations_,
stereoEps_,
stereoMaxSlope_);
UASSERT(refCorners3DTmp->size() == newCorners.size());
for(unsigned int i=0; i<newCorners.size(); ++i)
{
if(pcl::isFinite(refCorners3DTmp->at(i)) &&
(this->getMaxDepth() == 0.0f || refCorners3DTmp->at(i).z < this->getMaxDepth()))
{
newCorners3D->at(oi) = util3d::transformPoint(refCorners3DTmp->at(i), data.localTransform());
newCornersFiltered[oi] = newCorners[i];
++oi;
}
}
}
else
{
// depth
for(unsigned int i=0; i<newCorners.size(); ++i)
{
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true);
if(pcl::isFinite(pt) &&
(this->getMaxDepth() == 0.0f || pt.z < this->getMaxDepth()))
{
newCorners3D->at(oi) = util3d::transformPoint(pt, data.localTransform());
newCornersFiltered[oi] = newCorners[i];
++oi;
}
}
}
}
newCornersFiltered.resize(oi);
newCorners3D->resize(oi);
if((int)newCornersFiltered.size() >= this->getMinInliers())
{
refFrame_ = newLeftFrame;
refCorners_ = newCornersFiltered;
refCorners3D_ = newCorners3D;
}
else
{
UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...",
(int)newCornersFiltered.size(), (int)refCorners3D_->size(), this->getMinInliers());
output.setNull();
}
}
else
{
@@ -562,394 +537,4 @@ Transform OdometryOpticalFlow::computeTransformStereo(
return output;
}
Transform OdometryOpticalFlow::computeTransformRGBD(
const SensorData & data,
OdometryInfo * info)
{
UTimer timer;
Transform output;
double variance = 0;
int inliers = 0;
int correspondences = 0;
cv::Mat newFrame;
// convert to grayscale
if(data.image().channels() > 1)
{
cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY);
}
else
{
newFrame = data.image().clone();
}
std::vector<cv::Point2f> newCorners;
if(!refFrame_.empty() &&
(int)refCorners_.size() >= this->getMinInliers() &&
(int)refCorners3D_->size() >= this->getMinInliers())
{
std::vector<unsigned char> status;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
refFrame_,
newFrame,
refCorners_,
newCorners,
status,
err,
cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
if(this->isPnPEstimationUsed())
{
// find correspondences
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(refCorners_.size());
info->newCorners.resize(refCorners_.size());
}
UASSERT(refCorners_.size() == refCorners3D_->size());
UDEBUG("lastCorners3D_ = %d", refCorners3D_->size());
int flowInliers = 0;
std::vector<cv::Point3f> objectPoints(refCorners_.size());
std::vector<cv::Point2f> imagePoints(refCorners_.size());
std::vector<pcl::PointXYZ> image3DPoints(refCorners_.size());
int oi=0;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i])
{
if(pcl::isFinite(refCorners3D_->at(i)))
{
objectPoints[oi].x = refCorners3D_->at(i).x;
objectPoints[oi].y = refCorners3D_->at(i).y;
objectPoints[oi].z = refCorners3D_->at(i).z;
imagePoints[oi] = newCorners.at(i);
// new 3D points, used to compute variance
image3DPoints[oi] = pcl::PointXYZ(bad_point, bad_point, bad_point);
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true);
if(pcl::isFinite(pt) &&
(this->getMaxDepth() == 0.0f || (
uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
{
image3DPoints[oi] = util3d::transformPoint(pt, data.localTransform());
}
}
if(this->isInfoDataFilled() && info)
{
info->refCorners[oi] = refCorners_[i];
info->newCorners[oi] = newCorners[i];
}
++oi;
}
++flowInliers;
}
}
objectPoints.resize(oi);
imagePoints.resize(oi);
image3DPoints.resize(oi);
UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(oi);
info->newCorners.resize(oi);
}
correspondences = oi;
if(correspondences >= this->getMinInliers())
{
//PnPRansac
cv::Mat K = (cv::Mat_<double>(3,3) <<
data.fx(), 0, data.cx(),
0, data.fy(), data.cy(),
0, 0, 1);
Transform guess = (data.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
std::vector<int> inliersV;
cv::solvePnPRansac(objectPoints,
imagePoints,
K,
cv::Mat(),
rvec,
tvec,
true,
this->getIterations(),
this->getPnPReprojError(),
0,
inliersV,
this->getPnPFlags());
inliers = (int)inliersV.size();
if((int)inliersV.size() >= this->getMinInliers())
{
cv::Rodrigues(rvec, R);
Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
// make it incremental
output = (data.localTransform() * pnp).inverse();
UDEBUG("Odom transform = %s", output.prettyPrint().c_str());
// compute variance (like in PCL computeVariance() method of sac_model.h)
std::vector<float> errorSqrdDists(inliersV.size());
int ii=0;
for(unsigned int i=0; i<inliersV.size(); ++i)
{
pcl::PointXYZ & newPt = image3DPoints[inliersV[i]];
if(pcl::isFinite(newPt))
{
newPt = util3d::transformPoint(newPt, output);
const cv::Point3f & objPt = objectPoints[inliersV[i]];
errorSqrdDists[ii++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
}
}
errorSqrdDists.resize(ii);
if(errorSqrdDists.size())
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
variance = 2.1981 * median_error_sqr;
}
}
else
{
UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
}
if(this->isInfoDataFilled() && info)
{
info->cornerInliers = inliersV;
}
}
else
{
UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
}
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
correspondencesLast->resize(refCorners_.size());
correspondencesNew->resize(refCorners_.size());
int oi=0;
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(refCorners_.size());
info->newCorners.resize(refCorners_.size());
}
UASSERT(refCorners_.size() == refCorners3D_->size());
UDEBUG("lastCorners3D_ = %d", refCorners3D_->size());
int flowInliers = 0;
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] && pcl::isFinite(refCorners3D_->at(i)) &&
uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true);
if(pcl::isFinite(pt) &&
(this->getMaxDepth() == 0.0f || (
uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
{
pt = util3d::transformPoint(pt, data.localTransform());
correspondencesLast->at(oi) = refCorners3D_->at(i);
correspondencesNew->at(oi) = pt;
if(this->isInfoDataFilled() && info)
{
info->refCorners[oi] = refCorners_[i];
info->newCorners[oi] = newCorners[i];
}
++oi;
}
++flowInliers;
}
else if(status[i])
{
++flowInliers;
}
}
UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(oi);
info->newCorners.resize(oi);
}
correspondencesLast->resize(oi);
correspondencesNew->resize(oi);
correspondences = oi;
if(correspondences >= this->getMinInliers())
{
std::vector<int> inliersV;
UTimer timerRANSAC;
output = util3d::transformFromXYZCorrespondences(
correspondencesNew,
correspondencesLast,
this->getInlierDistance(),
this->getIterations(),
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
&inliersV,
&variance);
UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
inliers = (int)inliersV.size();
if(inliers < this->getMinInliers())
{
output.setNull();
UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
}
if(this->isInfoDataFilled() && info)
{
info->cornerInliers = inliersV;
}
}
else
{
UWARN("Not enough correspondences (%d)", correspondences);
}
}
}
else
{
//return Identity
output = Transform::getIdentity();
}
newCorners.clear();
if(!output.isNull())
{
// Copy or generate new keypoints
if(data.keypoints().size())
{
newCorners.resize(data.keypoints().size());
for(unsigned int i=0; i<data.keypoints().size(); ++i)
{
newCorners[i] = data.keypoints().at(i).pt;
}
}
else
{
// generate kpts
std::vector<cv::KeyPoint> newKtps;
cv::Rect roi = Feature2D::computeRoi(newFrame, this->getRoiRatios());
newKtps = feature2D_->generateKeypoints(newFrame, roi);
Feature2D::filterKeypointsByDepth(newKtps, data.depth(), this->getMaxDepth());
if(newKtps.size())
{
cv::KeyPoint::convert(newKtps, newCorners);
if(subPixWinSize_ > 0 && subPixIterations_ > 0)
{
cv::cornerSubPix(newFrame, newCorners,
cv::Size( subPixWinSize_, subPixWinSize_ ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
}
}
}
if((int)newCorners.size() > this->getMinInliers())
{
// get 3D corners for the extracted 2D corners (not the ones refined by Optical Flow)
pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
newCorners3D->resize(newCorners.size());
std::vector<cv::Point2f> newCornersFiltered(newCorners.size());
int oi=0;
for(unsigned int i=0; i<newCorners.size(); ++i)
{
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true);
if(pcl::isFinite(pt) &&
(this->getMaxDepth() == 0.0f || (
uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
{
pt = util3d::transformPoint(pt, data.localTransform());
newCorners3D->at(oi) = pt;
newCornersFiltered[oi] = newCorners[i];
++oi;
}
}
}
newCornersFiltered.resize(oi);
newCorners3D->resize(oi);
if((int)newCornersFiltered.size() > this->getMinInliers())
{
refFrame_ = newFrame;
refCorners_ = newCornersFiltered;
refCorners3D_ = newCorners3D;
}
else
{
UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...",
(int)newCornersFiltered.size(), (int)refCorners3D_->size(), this->getMinInliers());
output.setNull();
}
}
else
{
UWARN("Too low 2D corners (%d), ignoring new frame...",
(int)newCorners.size());
output.setNull();
}
}
if(info)
{
info->type = 1;
info->variance = variance;
info->inliers = inliers;
info->features = (int)newCorners.size();
info->matches = correspondences;
}
UINFO("Odom update time = %fs lost=%s inliers=%d/%d, variance=%f, new corners=%d",
timer.elapsed(),
output.isNull()?"true":"false",
inliers,
correspondences,
variance,
(int)newCorners.size());
return output;
}
} // namespace rtabmap

8
corelib/src/ParticleFilter.h
View File

@@ -132,7 +132,7 @@ public:
double filter(double val)
{
std::vector<double> weights(particles_.size());
std::vector<double> weights(particles_.size(), 1);
double sumWeights = 0;
for(unsigned int i=0; i<particles_.size(); ++i)
{
@@ -142,7 +142,11 @@ public:
// compute weight
double dist = fabs(particles_[i] - val);
//dist = sqrt(dist*dist);
weights[i] = exp(-lambda_*dist);
double w = exp(-lambda_*dist);
if(uIsFinite(w) && w > 0)
{
weights[i] = w;
}
sumWeights += weights[i];
}

4
corelib/src/Rtabmap.cpp
View File

@@ -2486,12 +2486,10 @@ void Rtabmap::dumpPoses(
#endif
if(fout)
{
Transform localTransformInv = Transform(0,0,0, -CV_PI/2, 0, -CV_PI/2).inverse();
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
Transform t = localTransformInv * (*iter).second;
// in camera frame
const float * p = (const float *)t.data();
const float * p = (const float *)(*iter).second.data();
fprintf(fout, "%f", p[0]);
for(int i=1; i<(*iter).second.size(); i++)

2
corelib/src/util2d.cpp
View File

@@ -162,7 +162,7 @@ cv::Mat disparityFromStereoCorrespondences(
{
float d = leftCorners[i].x - rightCorners[i].x;
float slope = fabs((leftCorners[i].y - rightCorners[i].y) / (leftCorners[i].x - rightCorners[i].x));
if(d > 0.0f && slope < maxSlope)
if(d > 0.0f && (maxSlope <= 0 || fabs(leftCorners[i].y-rightCorners[i].y) <= 1.0f || slope <= maxSlope))
{
disparity.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
}

47
corelib/src/util3d_features.cpp
View File

@@ -124,15 +124,46 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
int flowWinSize,
int flowMaxLevel,
int flowIterations,
double flowEps)
double flowEps,
double maxCorrespondencesSlope)
{
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
return generateKeypoints3DStereo(
leftCorners,
leftImage,
rightImage,
fx,
baseline,
cx,
cy,
transform,
flowWinSize,
flowMaxLevel,
flowIterations,
flowEps,
maxCorrespondencesSlope);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
const std::vector<cv::Point2f> & leftCorners,
const cv::Mat & leftImage,
const cv::Mat & rightImage,
float fx,
float baseline,
float cx,
float cy,
const Transform & transform,
int flowWinSize,
int flowMaxLevel,
int flowIterations,
double flowEps,
double maxCorrespondencesSlope)
{
UASSERT(!leftImage.empty() && !rightImage.empty() &&
leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols);
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
@@ -151,16 +182,18 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
UDEBUG("cv::calcOpticalFlowPyrLK() end");
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
keypoints3d->resize(keypoints.size());
keypoints3d->resize(leftCorners.size());
float bad_point = std::numeric_limits<float>::quiet_NaN ();
UASSERT(status.size() == keypoints.size());
UASSERT(status.size() == leftCorners.size());
for(unsigned int i=0; i<status.size(); ++i)
{
pcl::PointXYZ pt(bad_point, bad_point, bad_point);
if(status[i])
{
float disparity = leftCorners[i].x - rightCorners[i].x;
if(disparity > 0.0f)
float slope = fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x));
if(disparity > 0.0f &&
(maxCorrespondencesSlope <=0 || fabs(leftCorners[i].y-rightCorners[i].y) <= 1.0f || slope <= maxCorrespondencesSlope))
{
pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
leftCorners[i],

1
guilib/include/rtabmap/gui/DatabaseViewer.h
View File

@@ -105,6 +105,7 @@ private slots:
void resetConstraint();
void rejectConstraint();
void updateConstraintView();
void updateStereo();
private:
QString getIniFilePath() const;

122
guilib/src/DatabaseViewer.cpp
View File

@@ -102,6 +102,8 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->constraintsViewer->setCameraLockZ(false);
ui_->constraintsViewer->setCameraFree();
ui_->graphicsView_stereo->setAlpha(255);
this->readSettings();
if(RTABMAP_NONFREE == 0)
@@ -215,6 +217,20 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->spinBox_projDecimation, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
connect(ui_->doubleSpinBox_projMaxDepth, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
connect(ui_->spinBox_stereo_flowIterations, SIGNAL(valueChanged(int)), this, SLOT(updateStereo()));
connect(ui_->spinBox_stereo_flowMaxLevel, SIGNAL(valueChanged(int)), this, SLOT(updateStereo()));
connect(ui_->spinBox_stereo_flowWinSize, SIGNAL(valueChanged(int)), this, SLOT(updateStereo()));
connect(ui_->spinBox_stereo_gfttBlockSize, SIGNAL(valueChanged(int)), this, SLOT(updateStereo()));
connect(ui_->doubleSpinBox_stereo_flowEps, SIGNAL(valueChanged(double)), this, SLOT(updateStereo()));
connect(ui_->doubleSpinBox_stereo_gfttMinDistance, SIGNAL(valueChanged(double)), this, SLOT(updateStereo()));
connect(ui_->doubleSpinBox_stereo_gfttQuality, SIGNAL(valueChanged(double)), this, SLOT(updateStereo()));
connect(ui_->doubleSpinBox_stereo_maxSlope, SIGNAL(valueChanged(double)), this, SLOT(updateStereo()));
connect(ui_->checkBox_stereo_subpix, SIGNAL(stateChanged(int)), this, SLOT(updateStereo()));
ui_->label_stereo_inliers_name->setStyleSheet("QLabel {color : blue; }");
ui_->label_stereo_flowOutliers_name->setStyleSheet("QLabel {color : red; }");
ui_->label_stereo_slopeOutliers_name->setStyleSheet("QLabel {color : yellow; }");
ui_->label_stereo_disparityOutliers_name->setStyleSheet("QLabel {color : magenta; }");
// connect configuration changed
connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
@@ -263,6 +279,16 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->doubleSpinBox_detectMore_radius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_detectMore_angle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
//stereo parameters
connect(ui_->spinBox_stereo_flowIterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->spinBox_stereo_flowMaxLevel, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->spinBox_stereo_flowWinSize, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->spinBox_stereo_gfttBlockSize, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_stereo_flowEps, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_stereo_gfttMinDistance, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_stereo_gfttQuality, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_stereo_maxSlope, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->checkBox_stereo_subpix, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
// dockwidget
QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
for(int i=0; i<dockWidgets.size(); ++i)
@@ -387,6 +413,19 @@ void DatabaseViewer::readSettings()
ui_->spinBox_detectMore_iterations->setValue(settings.value("detectMoreIterations", ui_->spinBox_detectMore_iterations->value()).toInt());
settings.endGroup();
//Stereo parameters
settings.beginGroup("stereo");
ui_->spinBox_stereo_flowIterations->setValue(settings.value("flowIterations", ui_->spinBox_stereo_flowIterations->value()).toInt());
ui_->spinBox_stereo_flowMaxLevel->setValue(settings.value("flowMaxLevel", ui_->spinBox_stereo_flowMaxLevel->value()).toInt());
ui_->spinBox_stereo_flowWinSize->setValue(settings.value("flowWinSize", ui_->spinBox_stereo_flowWinSize->value()).toInt());
ui_->spinBox_stereo_gfttBlockSize->setValue(settings.value("gfttBlockSize", ui_->spinBox_stereo_gfttBlockSize->value()).toInt());
ui_->doubleSpinBox_stereo_flowEps->setValue(settings.value("flowEps", ui_->doubleSpinBox_stereo_flowEps->value()).toDouble());
ui_->doubleSpinBox_stereo_gfttMinDistance->setValue(settings.value("gfttMinDistance", ui_->doubleSpinBox_stereo_gfttMinDistance->value()).toDouble());
ui_->doubleSpinBox_stereo_gfttQuality->setValue(settings.value("gfttQuality", ui_->doubleSpinBox_stereo_gfttQuality->value()).toDouble());
ui_->doubleSpinBox_stereo_maxSlope->setValue(settings.value("maxSlope", ui_->doubleSpinBox_stereo_maxSlope->value()).toDouble());
ui_->checkBox_stereo_subpix->setChecked(settings.value("subpix", ui_->checkBox_stereo_subpix->isChecked()).toBool());
settings.endGroup();
settings.endGroup(); // DatabaseViewer
}
@@ -468,6 +507,19 @@ void DatabaseViewer::writeSettings()
settings.setValue("detectMoreIterations", ui_->spinBox_detectMore_iterations->value());
settings.endGroup();
//Stereo parameters
settings.beginGroup("stereo");
settings.setValue("flowIterations", ui_->spinBox_stereo_flowIterations->value());
settings.setValue("flowMaxLevel", ui_->spinBox_stereo_flowMaxLevel->value());
settings.setValue("flowWinSize", ui_->spinBox_stereo_flowWinSize->value());
settings.setValue("gfttBlockSize", ui_->spinBox_stereo_gfttBlockSize->value());
settings.setValue("flowEps", ui_->doubleSpinBox_stereo_flowEps->value());
settings.setValue("gfttMinDistance", ui_->doubleSpinBox_stereo_gfttMinDistance->value());
settings.setValue("gfttQuality", ui_->doubleSpinBox_stereo_gfttQuality->value());
settings.setValue("maxSlope", ui_->doubleSpinBox_stereo_maxSlope->value());
settings.setValue("subpix", ui_->checkBox_stereo_subpix->isChecked());
settings.endGroup();
settings.endGroup(); // DatabaseViewer
this->setWindowModified(false);
@@ -1535,6 +1587,11 @@ void DatabaseViewer::update(int value,
{
this->updateStereo(&data);
}
else
{
ui_->stereoViewer->clear();
ui_->graphicsView_stereo->clear();
}
// 3d view
if(view3D->isVisible() && !data.getDepthRaw().empty())
@@ -1688,6 +1745,16 @@ void DatabaseViewer::update(int value,
}
}
void DatabaseViewer::updateStereo()
{
if(ui_->horizontalSlider_A->maximum())
{
int id = ids_.at(ui_->horizontalSlider_A->value());
Signature data = memory_->getSignatureData(id, true);
updateStereo(&data);
}
}
void DatabaseViewer::updateStereo(const Signature * data)
{
if(data && ui_->dockWidget_stereoView->isVisible() && !data->getImageRaw().empty() && !data->getDepthRaw().empty() && data->getDepthRaw().type() == CV_8UC1)
@@ -1708,8 +1775,10 @@ void DatabaseViewer::updateStereo(const Signature * data)
std::vector<cv::KeyPoint> kpts;
cv::Rect roi = Feature2D::computeRoi(leftMono, "0.03 0.03 0.04 0.04");
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "1000"));
parameters.insert(ParametersPair(Parameters::kGFTTMinDistance(), "5"));
parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0"));
parameters.insert(ParametersPair(Parameters::kGFTTMinDistance(), uNumber2Str(ui_->doubleSpinBox_stereo_gfttMinDistance->value())));
parameters.insert(ParametersPair(Parameters::kGFTTQualityLevel(), uNumber2Str(ui_->doubleSpinBox_stereo_gfttQuality->value())));
parameters.insert(ParametersPair(Parameters::kGFTTBlockSize(), uNumber2Str(ui_->spinBox_stereo_gfttBlockSize->value())));
Feature2D::Type type = Feature2D::kFeatureGfttBrief;
Feature2D * kptDetector = Feature2D::create(type, parameters);
kpts = kptDetector->generateKeypoints(leftMono, roi);
@@ -1720,6 +1789,19 @@ void DatabaseViewer::updateStereo(const Signature * data)
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(kpts, leftCorners);
int subPixWinSize = 3;
int subPixIterations = 30;
double subPixEps = 0.02;
if(ui_->checkBox_stereo_subpix->isChecked())
{
UDEBUG("cv::cornerSubPix() begin");
cv::cornerSubPix(leftMono, leftCorners,
cv::Size( subPixWinSize, subPixWinSize ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations, subPixEps ) );
UDEBUG("cv::cornerSubPix() end");
}
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
@@ -1731,8 +1813,8 @@ void DatabaseViewer::updateStereo(const Signature * data)
rightCorners,
status,
err,
cv::Size(Parameters::defaultStereoWinSize(), Parameters::defaultStereoWinSize()), Parameters::defaultStereoMaxLevel(),
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, Parameters::defaultStereoIterations(), Parameters::defaultStereoEps()));
cv::Size(ui_->spinBox_stereo_flowWinSize->value(), ui_->spinBox_stereo_flowWinSize->value()), ui_->spinBox_stereo_flowMaxLevel->value(),
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, ui_->spinBox_stereo_flowIterations->value(), ui_->doubleSpinBox_stereo_flowEps->value()));
float timeFlow = timer.ticks();
@@ -1741,6 +1823,10 @@ void DatabaseViewer::updateStereo(const Signature * data)
float bad_point = std::numeric_limits<float>::quiet_NaN ();
UASSERT(status.size() == kpts.size());
int oi = 0;
int inliers = 0;
int flowOutliers= 0;
int slopeOutliers= 0;
int negativeDisparityOutliers = 0;
for(unsigned int i=0; i<status.size(); ++i)
{
pcl::PointXYZ pt(bad_point, bad_point, bad_point);
@@ -1749,7 +1835,7 @@ void DatabaseViewer::updateStereo(const Signature * data)
float disparity = leftCorners[i].x - rightCorners[i].x;
if(disparity > 0.0f)
{
if(fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x)) < Parameters::defaultStereoMaxSlope())
if(fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x)) < ui_->doubleSpinBox_stereo_maxSlope->value())
{
pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
leftCorners[i],
@@ -1759,23 +1845,32 @@ void DatabaseViewer::updateStereo(const Signature * data)
if(pcl::isFinite(tmpPt))
{
pt = pcl::transformPoint(tmpPt, data->getLocalTransform().toEigen3f());
if(fabs(pt.x) > 2 || fabs(pt.y) > 2 || fabs(pt.z) > 2)
{
status[i] = 100; //blue
}
status[i] = 100; //blue
++inliers;
cloud->at(oi++) = pt;
}
}
else if(fabs(leftCorners[i].y-rightCorners[i].y) <=1.0f)
{
status[i] = 110; //cyan
++inliers;
}
else
{
status[i] = 101; //yellow
++slopeOutliers;
}
}
else
{
status[i] = 102; //magenta
++negativeDisparityOutliers;
}
}
else
{
++flowOutliers;
}
}
cloud->resize(oi);
@@ -1786,6 +1881,11 @@ void DatabaseViewer::updateStereo(const Signature * data)
ui_->stereoViewer->addOrUpdateCloud("stereo", cloud);
ui_->stereoViewer->update();
ui_->label_stereo_inliers->setNum(inliers);
ui_->label_stereo_flowOutliers->setNum(flowOutliers);
ui_->label_stereo_slopeOutliers->setNum(slopeOutliers);
ui_->label_stereo_disparityOutliers->setNum(negativeDisparityOutliers);
std::vector<cv::KeyPoint> rightKpts;
cv::KeyPoint::convert(rightCorners, rightKpts);
std::vector<cv::DMatch> good_matches(kpts.size());
@@ -1832,6 +1932,10 @@ void DatabaseViewer::updateStereo(const Signature * data)
{
c = Qt::magenta;
}
else if(status[i] == 110)
{
c = Qt::cyan;
}
ui_->graphicsView_stereo->addLine(
kpts[i].pt.x,
kpts[i].pt.y,

35
guilib/src/MainWindow.cpp
View File

@@ -880,20 +880,21 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
{
//draw lines
UASSERT(info.refCorners.size() == info.newCorners.size());
for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
std::set<int> inliers(info.cornerInliers.begin(), info.cornerInliers.end());
for(unsigned int i=0; i<info.refCorners.size(); ++i)
{
if(_ui->imageView_odometry->isFeaturesShown())
if(_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end())
{
_ui->imageView_odometry->setFeatureColor(info.cornerInliers[i], Qt::green); // inliers
_ui->imageView_odometry->setFeatureColor(i, Qt::green); // inliers
}
if(_ui->imageView_odometry->isLinesShown())
{
_ui->imageView_odometry->addLine(
info.refCorners[info.cornerInliers[i]].x,
info.refCorners[info.cornerInliers[i]].y,
info.newCorners[info.cornerInliers[i]].x,
info.newCorners[info.cornerInliers[i]].y,
Qt::blue);
info.refCorners[i].x,
info.refCorners[i].y,
info.newCorners[i].x,
info.newCorners[i].y,
inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow);
}
}
}
@@ -975,6 +976,10 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
_ui->statsToolBox->updateStat("Odometry/Interval/ms", (float)data.id(), info.interval*1000.f);
_ui->statsToolBox->updateStat("Odometry/Speed/kph", (float)data.id(), x/info.interval*3.6f);
}
if(info.distanceTravelled > 0)
{
_ui->statsToolBox->updateStat("Odometry/Distance/m", (float)data.id(), info.distanceTravelled);
}
_ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)data.id(), time.elapsed()*1000.0);
_processingOdometry = false;
@@ -993,7 +998,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
int loopMapId = uValue(stat.getMapIds(), stat.loopClosureId(), uValue(stat.getMapIds(), stat.localLoopClosureId(), -1));
_ui->label_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId));
_ui->label_matchId->clear();
if(stat.extended())
{
@@ -1252,6 +1256,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
_ui->label_stats_loopClosuresDetected->setText(QString::number(_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
_ui->label_matchId->setText(QString("Match ID = %1 [%2]").arg(stat.loopClosureId()).arg(loopMapId));
}
else
{
_ui->label_matchId->clear();
}
float elapsedTime = static_cast<float>(totalTime.elapsed());
UINFO("Updating GUI time = %fs", elapsedTime/1000.0f);
_ui->statsToolBox->updateStat("/Gui refresh stats/ms", stat.refImageId(), elapsedTime);
@@ -2902,7 +2910,14 @@ void MainWindow::startDetection()
if(_dataRecorder)
{
UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent");
if(_camera)
{
UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent");
}
else if(_dbReader)
{
UEventsManager::createPipe(_dbReader, _dataRecorder, "CameraEvent");
}
}
_lastOdomPose.setNull();

346
guilib/src/ui/DatabaseViewer.ui
View File

@@ -50,7 +50,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>172</width>
<width>196</width>
<height>184</height>
</rect>
</property>
@@ -200,6 +200,9 @@
</item>
<item>
<widget class="QSlider" name="horizontalSlider_A">
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
@@ -233,7 +236,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>172</width>
<width>195</width>
<height>184</height>
</rect>
</property>
@@ -383,6 +386,9 @@
</item>
<item>
<widget class="QSlider" name="horizontalSlider_B">
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
@@ -479,6 +485,9 @@
</item>
<item row="0" column="1">
<widget class="QSlider" name="horizontalSlider_neighbors">
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
@@ -496,6 +505,9 @@
</item>
<item row="1" column="1">
<widget class="QSlider" name="horizontalSlider_loops">
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
@@ -654,6 +666,9 @@
</item>
<item>
<widget class="QSlider" name="horizontalSlider_iterations">
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
</property>
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
@@ -783,14 +798,14 @@
<item>
<widget class="QToolBox" name="toolBox">
<property name="currentIndex">
<number>1</number>
<number>5</number>
</property>
<widget class="QWidget" name="page">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>315</width>
<width>312</width>
<height>314</height>
</rect>
</property>
@@ -1009,7 +1024,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-12</y>
<y>0</y>
<width>351</width>
<height>355</height>
</rect>
@@ -1510,7 +1525,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>315</width>
<width>248</width>
<height>319</height>
</rect>
</property>
@@ -1704,8 +1719,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>330</width>
<height>186</height>
<width>201</width>
<height>126</height>
</rect>
</property>
<attribute name="label">
@@ -1799,6 +1814,217 @@
</item>
</layout>
</widget>
<widget class="QWidget" name="page_6">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>283</width>
<height>322</height>
</rect>
</property>
<attribute name="label">
<string>Stereo correspondences</string>
</attribute>
<layout class="QGridLayout" name="gridLayout_13">
<item row="2" column="0">
<widget class="QSpinBox" name="spinBox_stereo_gfttBlockSize">
<property name="suffix">
<string> pixels</string>
</property>
<property name="minimum">
<number>1</number>
</property>
<property name="value">
<number>3</number>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_stereo_gfttQuality">
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.001000000000000</double>
</property>
<property name="value">
<double>0.010000000000000</double>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_59">
<property name="text">
<string>Optical Flow eps</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_55">
<property name="text">
<string>GFTT block size</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QSpinBox" name="spinBox_stereo_flowWinSize">
<property name="suffix">
<string> pixels</string>
</property>
<property name="minimum">
<number>3</number>
</property>
<property name="value">
<number>16</number>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_56">
<property name="text">
<string>Optical Flow win size</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QSpinBox" name="spinBox_stereo_flowMaxLevel">
<property name="minimum">
<number>1</number>
</property>
<property name="value">
<number>3</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_61">
<property name="text">
<string>GFTT min distance</string>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_stereo_flowEps">
<property name="decimals">
<number>4</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.010000000000000</double>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_57">
<property name="text">
<string>Optical Flow max level</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_58">
<property name="text">
<string>Optical Flow iterations</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_54">
<property name="text">
<string>GFTT quality level</string>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QSpinBox" name="spinBox_stereo_flowIterations">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>30</number>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_stereo_maxSlope">
<property name="decimals">
<number>2</number>
</property>
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.100000000000000</double>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_stereo_gfttMinDistance">
<property name="suffix">
<string> pixels</string>
</property>
<property name="decimals">
<number>1</number>
</property>
<property name="value">
<double>5.000000000000000</double>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_62">
<property name="text">
<string>Sub pixel</string>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_60">
<property name="text">
<string>Matches max slope</string>
</property>
</widget>
</item>
<item row="9" column="1">
<spacer name="verticalSpacer_6">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item row="8" column="0">
<widget class="QCheckBox" name="checkBox_stereo_subpix">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</widget>
</item>
</layout>
@@ -1812,12 +2038,110 @@
<number>4</number>
</attribute>
<widget class="QWidget" name="dockWidgetContents_5">
<layout class="QHBoxLayout" name="horizontalLayout_6" stretch="1,1">
<layout class="QVBoxLayout" name="verticalLayout_11" stretch="1,0">
<property name="spacing">
<number>0</number>
</property>
<property name="margin">
<number>0</number>
</property>
<item>
<widget class="rtabmap::ImageView" name="graphicsView_stereo" native="true"/>
<layout class="QHBoxLayout" name="horizontalLayout_9" stretch="1,1">
<item>
<widget class="rtabmap::ImageView" name="graphicsView_stereo" native="true"/>
</item>
<item>
<widget class="rtabmap::CloudViewer" name="stereoViewer" native="true"/>
</item>
</layout>
</item>
<item>
<widget class="rtabmap::CloudViewer" name="stereoViewer" native="true"/>
<layout class="QHBoxLayout" name="horizontalLayout_6">
<property name="spacing">
<number>6</number>
</property>
<item>
<widget class="QLabel" name="label_stereo_inliers_name">
<property name="text">
<string>Inliers:</string>
</property>
<property name="textFormat">
<enum>Qt::RichText</enum>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_stereo_inliers">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_stereo_slopeOutliers_name">
<property name="text">
<string>Slope outliers:</string>
</property>
<property name="textFormat">
<enum>Qt::RichText</enum>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_stereo_slopeOutliers">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_stereo_disparityOutliers_name">
<property name="text">
<string>Negative disparity outliers:</string>
</property>
<property name="textFormat">
<enum>Qt::RichText</enum>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_stereo_disparityOutliers">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_stereo_flowOutliers_name">
<property name="text">
<string>Flow outliers:</string>
</property>
<property name="textFormat">
<enum>Qt::RichText</enum>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_stereo_flowOutliers">
<property name="text">
<string>0</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
</layout>
</widget>

9
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-613</y>
<y>-450</y>
<width>755</width>
<height>1591</height>
</rect>
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>3</number>
<number>23</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -7222,11 +7222,14 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<string> m</string>
</property>
<property name="decimals">
<number>2</number>
<number>0</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>999.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>