i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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;} bool isPnPEstimationUsed() const {return _pnpEstimation;}
double getPnPReprojError() const {return _pnpReprojError;} double getPnPReprojError() const {return _pnpReprojError;}
int getPnPFlags() const {return _pnpFlags;} int getPnPFlags() const {return _pnpFlags;}
const Transform & previousTransform() const {return previousTransform_;}
private: private:
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0; virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
@@ -89,6 +90,8 @@ private:
Transform _pose; Transform _pose;
int _resetCurrentCount; int _resetCurrentCount;
double previousStamp_; double previousStamp_;
Transform previousTransform_;
float distanceTravelled_;
std::vector<ParticleFilter *> filters_; std::vector<ParticleFilter *> filters_;
@@ -133,9 +136,7 @@ public:
private: private:
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0); 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: private:
//Parameters: //Parameters:
int flowWinSize_; int flowWinSize_;
@@ -156,7 +157,6 @@ private:
Feature2D * feature2D_; Feature2D * feature2D_;
cv::Mat refFrame_; cv::Mat refFrame_;
cv::Mat refRightFrame_;
std::vector<cv::Point2f> refCorners_; std::vector<cv::Point2f> refCorners_;
pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_; pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_;
}; };

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

@@ -43,6 +43,7 @@ public:
timeEstimation(-1), timeEstimation(-1),
stamp(0), stamp(0),
interval(0), interval(0),
distanceTravelled(0),
type(-1) type(-1)
{} {}
bool lost; bool lost;
@@ -57,6 +58,7 @@ public:
double interval; double interval;
Transform transform; Transform transform;
Transform transformFiltered; Transform transformFiltered;
float distanceTravelled;
int type; // 0=BOW, 1=Optical Flow, 2=ICP 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 flowWinSize = 9,
int flowMaxLevel = 4, int flowMaxLevel = 4,
int flowIterations = 20, 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( std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3DMono(
const std::multimap<int, cv::KeyPoint> & kpts, 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/core/OdometryInfo.h"
#include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
#include "ParticleFilter.h" #include "ParticleFilter.h"
namespace rtabmap { namespace rtabmap {
@@ -53,7 +54,9 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_pnpReprojError(Parameters::defaultOdomPnPReprojError()), _pnpReprojError(Parameters::defaultOdomPnPReprojError()),
_pnpFlags(Parameters::defaultOdomPnPFlags()), _pnpFlags(Parameters::defaultOdomPnPFlags()),
_resetCurrentCount(0), _resetCurrentCount(0),
previousStamp_(0) previousStamp_(0),
previousTransform_(Transform::getIdentity()),
distanceTravelled_(0)
{ {
Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown); Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers); Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers);
@@ -106,8 +109,10 @@ Odometry::~Odometry()
void Odometry::reset(const Transform & initialPose) void Odometry::reset(const Transform & initialPose)
{ {
previousTransform_.setIdentity();
_resetCurrentCount = 0; _resetCurrentCount = 0;
previousStamp_ = 0; previousStamp_ = 0;
distanceTravelled_ = 0;
if(_force2D || filters_.size()) if(_force2D || filters_.size())
{ {
float x,y,z, roll,pitch,yaw; 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->interval = data.stamp() - previousStamp_;
info->transform = t; info->transform = t;
} }
previousTransform_.setIdentity();
previousStamp_ = data.stamp(); previousStamp_ = data.stamp();
if(!t.isNull()) if(!t.isNull())
@@ -192,15 +199,27 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
if(filters_.size()) if(filters_.size())
{ {
UASSERT(filters_.size()==6); UASSERT(filters_.size()==6);
x = filters_[0]->filter(x); if(_pose.isIdentity())
y = filters_[1]->filter(y);
yaw = filters_[5]->filter(yaw);
if(!_force2D)
{ {
z = filters_[2]->filter(z); filters_[0]->init(x);
roll = filters_[3]->filter(roll); filters_[1]->init(y);
pitch = filters_[4]->filter(pitch); 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) if(info)
@@ -208,6 +227,10 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
info->timeParticleFiltering = time.ticks(); 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); t = Transform(x,y,_force2D?0:z, _force2D?0:roll,_force2D?0:pitch,yaw);
if(info) 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 return _pose *= t; // updated
} }
else if(_resetCurrentCount > 0) 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_transforms.h"
#include "rtabmap/core/util3d.h" #include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_registration.h" #include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_features.h"
#include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h" #include "rtabmap/utilite/UConversion.h"
@@ -120,29 +121,6 @@ void OdometryOpticalFlow::reset(const Transform & initialPose)
Transform OdometryOpticalFlow::computeTransform( Transform OdometryOpticalFlow::computeTransform(
const SensorData & data, const SensorData & data,
OdometryInfo * info) 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; UTimer timer;
Transform output; Transform output;
@@ -151,6 +129,11 @@ Transform OdometryOpticalFlow::computeTransformStereo(
int inliers = 0; int inliers = 0;
int correspondences = 0; int correspondences = 0;
if(info)
{
info->type = 1;
}
cv::Mat newLeftFrame; cv::Mat newLeftFrame;
// convert to grayscale // convert to grayscale
if(data.image().channels() > 1) if(data.image().channels() > 1)
@@ -161,17 +144,50 @@ Transform OdometryOpticalFlow::computeTransformStereo(
{ {
newLeftFrame = data.image().clone(); newLeftFrame = data.image().clone();
} }
cv::Mat newRightFrame = data.rightImage().clone();
std::vector<cv::Point2f> newCorners; std::vector<cv::Point2f> newCorners;
UDEBUG("lastCorners_.size()=%d lastFrame_=%d lastRightFrame_=%d", (int)refCorners_.size(), refFrame_.empty()?0:1, refRightFrame_.empty()?0:1); UDEBUG("lastCorners_.size()=%d lastFrame_=%d depthRight=%d",
if(!refFrame_.empty() && !refRightFrame_.empty() && refCorners_.size()) (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 // Find features in the new left image
std::vector<unsigned char> status; std::vector<unsigned char> status;
std::vector<float> err; std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin"); UDEBUG("cv::calcOpticalFlowPyrLK() begin");
int winSize = (newCorners.size()||!flowGuessByMotion)?flowWinSize_:(flowWinSize_*2);
cv::calcOpticalFlowPyrLK( cv::calcOpticalFlowPyrLK(
refFrame_, refFrame_,
newLeftFrame, newLeftFrame,
@@ -179,155 +195,54 @@ Transform OdometryOpticalFlow::computeTransformStereo(
newCorners, newCorners,
status, status,
err, 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::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"); 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()); std::vector<cv::Point2f> newCornersKept(status.size());
int ki = 0; int ki = 0;
for(unsigned int i=0; i<status.size(); ++i) for(unsigned int i=0; i<status.size(); ++i)
{ {
if(status[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]; newCornersKept[ki] = newCorners[i];
++ki; ++ki;
} }
} }
lastCornersKept.resize(ki); refCorners3DKept->resize(ki);
objectPointsKept.resize(ki);
refCornersKept.resize(ki);
newCornersKept.resize(ki); newCornersKept.resize(ki);
if(ki && ki >= this->getMinInliers()) 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()) if(this->isPnPEstimationUsed())
{ {
// find correspondences // find correspondences
if(this->isInfoDataFilled() && info) if(this->isInfoDataFilled() && info)
{ {
info->refCorners.resize(statusLast.size()); info->refCorners = refCornersKept;
info->newCorners.resize(statusLast.size()); info->newCorners = newCornersKept;
} }
int flowInliers = 0; correspondences = refCornersKept.size();
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;
if(correspondences >= this->getMinInliers()) if(correspondences >= this->getMinInliers())
{ {
//PnPRansac //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; std::vector<int> inliersV;
cv::solvePnPRansac(objectPoints, cv::solvePnPRansac(
imagePoints, objectPointsKept,
newCornersKept,
K, K,
cv::Mat(), cv::Mat(),
rvec, rvec,
@@ -339,39 +254,17 @@ Transform OdometryOpticalFlow::computeTransformStereo(
inliersV, inliersV,
this->getPnPFlags()); 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(); inliers = (int)inliersV.size();
if((int)inliersV.size() >= this->getMinInliers()) 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 // make it incremental
output = (data.localTransform() * pnp).inverse(); output = (data.localTransform() * pnp).inverse();
variance = 1; // FIXME, is there a way to compute a variance from the PNP approach?
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 else
{ {
@@ -390,57 +283,79 @@ Transform OdometryOpticalFlow::computeTransformStereo(
} }
else else
{ {
UDEBUG("Getting correspondences begin");
// Get 3D correspondences // 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>); pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
correspondencesLast->resize(statusLast.size()); correspondencesRef->resize(newCornersKept.size());
correspondencesNew->resize(statusLast.size()); correspondencesNew->resize(newCornersKept.size());
int oi = 0;
if(this->isInfoDataFilled() && info) if(this->isInfoDataFilled() && info)
{ {
info->refCorners.resize(statusLast.size()); info->refCorners.resize(newCornersKept.size());
info->newCorners.resize(statusLast.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]) // stereo
{ pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D = util3d::generateKeypoints3DStereo(
float lastDisparity = lastCornersKept[i].x - lastCornersKeptRight[i].x; newCornersKept,
float newDisparity = newCornersKept[i].x - newCornersKeptRight[i].x; newLeftFrame,
float lastSlope = fabs((lastCornersKept[i].y-lastCornersKeptRight[i].y) / (lastCornersKept[i].x-lastCornersKeptRight[i].x)); data.rightImage(),
float newSlope = fabs((newCornersKept[i].y-newCornersKeptRight[i].y) / (newCornersKept[i].x-newCornersKeptRight[i].x)); data.fx(),
if(lastDisparity > 0.0f && newDisparity > 0.0f && data.baseline(),
lastSlope < stereoMaxSlope_ && newSlope < stereoMaxSlope_) data.cx(),
{ data.cy(),
pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D( Transform::getIdentity(),
lastCornersKept[i], stereoWinSize_,
lastDisparity, stereoMaxLevel_,
data.cx(), data.cy(), data.fx(), data.baseline()); stereoIterations_,
pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D( stereoEps_,
newCornersKept[i], stereoMaxSlope_);
newDisparity,
data.cx(), data.cy(), data.fx(), data.baseline());
if(pcl::isFinite(lastPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth())) && UASSERT(newCorners3D->size() == refCorners3DKept->size());
pcl::isFinite(newPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth()))) 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! //Add 3D correspondences!
lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform()); correspondencesRef->at(oi) = refCorners3DKept->at(i);
newPt3D = util3d::transformPoint(newPt3D, data.localTransform()); correspondencesNew->at(oi) = util3d::transformPoint(pt, data.localTransform());
correspondencesLast->at(oi) = lastPt3D;
correspondencesNew->at(oi) = newPt3D;
if(this->isInfoDataFilled() && info) if(this->isInfoDataFilled() && info)
{ {
info->refCorners[oi] = lastCornersKept[i]; info->refCorners[oi] = refCornersKept[i];
info->newCorners[oi] = newCornersKept[i]; info->newCorners[oi] = newCornersKept[i];
} }
++oi; ++oi;
} }
} }
} }
}// end loop }
correspondencesLast->resize(oi); correspondencesRef->resize(oi);
correspondencesNew->resize(oi); correspondencesNew->resize(oi);
if(this->isInfoDataFilled() && info) if(this->isInfoDataFilled() && info)
{ {
@@ -448,8 +363,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
info->newCorners.resize(oi); info->newCorners.resize(oi);
} }
correspondences = oi; correspondences = oi;
refCorners3D_ = correspondencesNew; UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)newCornersKept.size());
UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)statusLast.size());
if(correspondences >= this->getMinInliers()) if(correspondences >= this->getMinInliers())
{ {
@@ -457,7 +371,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
UTimer timerRANSAC; UTimer timerRANSAC;
Transform t = util3d::transformFromXYZCorrespondences( Transform t = util3d::transformFromXYZCorrespondences(
correspondencesNew, correspondencesNew,
correspondencesLast, correspondencesRef,
this->getInlierDistance(), this->getInlierDistance(),
this->getIterations(), this->getIterations(),
this->getRefineIterations()>0, 3.0, this->getRefineIterations(), this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
@@ -499,11 +413,7 @@ Transform OdometryOpticalFlow::computeTransformStereo(
// Copy or generate new keypoints // Copy or generate new keypoints
if(data.keypoints().size()) if(data.keypoints().size())
{ {
newCorners.resize(data.keypoints().size()); cv::KeyPoint::convert(data.keypoints(), newCorners);
for(unsigned int i=0; i<data.keypoints().size(); ++i)
{
newCorners[i] = data.keypoints().at(i).pt;
}
} }
else else
{ {
@@ -528,11 +438,76 @@ Transform OdometryOpticalFlow::computeTransformStereo(
} }
} }
if((int)newCorners.size() > this->getMinInliers()) if((int)newCorners.size() >= this->getMinInliers())
{ {
refFrame_ = newLeftFrame; pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
refRightFrame_ = newRightFrame; newCorners3D->resize(newCorners.size());
refCorners_ = newCorners; 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 else
{ {
@@ -562,394 +537,4 @@ Transform OdometryOpticalFlow::computeTransformStereo(
return output; 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 } // namespace rtabmap

8
corelib/src/ParticleFilter.h
View File

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

4
corelib/src/Rtabmap.cpp
View File

@@ -2486,12 +2486,10 @@ void Rtabmap::dumpPoses(
#endif #endif
if(fout) 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) for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{ {
Transform t = localTransformInv * (*iter).second;
// in camera frame // in camera frame
const float * p = (const float *)t.data(); const float * p = (const float *)(*iter).second.data();
fprintf(fout, "%f", p[0]); fprintf(fout, "%f", p[0]);
for(int i=1; i<(*iter).second.size(); i++) 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 d = leftCorners[i].x - rightCorners[i].x;
float slope = fabs((leftCorners[i].y - rightCorners[i].y) / (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; 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 flowWinSize,
int flowMaxLevel, int flowMaxLevel,
int flowIterations, 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() && UASSERT(!leftImage.empty() && !rightImage.empty() &&
leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 && leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols); 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 // Find features in the new left image
std::vector<unsigned char> status; std::vector<unsigned char> status;
std::vector<float> err; std::vector<float> err;
@@ -151,16 +182,18 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
UDEBUG("cv::calcOpticalFlowPyrLK() end"); UDEBUG("cv::calcOpticalFlowPyrLK() end");
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>); 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 (); 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) for(unsigned int i=0; i<status.size(); ++i)
{ {
pcl::PointXYZ pt(bad_point, bad_point, bad_point); pcl::PointXYZ pt(bad_point, bad_point, bad_point);
if(status[i]) if(status[i])
{ {
float disparity = leftCorners[i].x - rightCorners[i].x; 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( pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
leftCorners[i], leftCorners[i],

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

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

122
guilib/src/DatabaseViewer.cpp
View File

@@ -102,6 +102,8 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->constraintsViewer->setCameraLockZ(false); ui_->constraintsViewer->setCameraLockZ(false);
ui_->constraintsViewer->setCameraFree(); ui_->constraintsViewer->setCameraFree();
ui_->graphicsView_stereo->setAlpha(255);
this->readSettings(); this->readSettings();
if(RTABMAP_NONFREE == 0) if(RTABMAP_NONFREE == 0)
@@ -215,6 +217,20 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->spinBox_projDecimation, SIGNAL(editingFinished()), this, SLOT(updateGrid())); connect(ui_->spinBox_projDecimation, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
connect(ui_->doubleSpinBox_projMaxDepth, 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 configuration changed
connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified())); 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_radius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_detectMore_angle, 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())); 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 // dockwidget
QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>(); QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
for(int i=0; i<dockWidgets.size(); ++i) 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()); ui_->spinBox_detectMore_iterations->setValue(settings.value("detectMoreIterations", ui_->spinBox_detectMore_iterations->value()).toInt());
settings.endGroup(); 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 settings.endGroup(); // DatabaseViewer
} }
@@ -468,6 +507,19 @@ void DatabaseViewer::writeSettings()
settings.setValue("detectMoreIterations", ui_->spinBox_detectMore_iterations->value()); settings.setValue("detectMoreIterations", ui_->spinBox_detectMore_iterations->value());
settings.endGroup(); 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 settings.endGroup(); // DatabaseViewer
this->setWindowModified(false); this->setWindowModified(false);
@@ -1535,6 +1587,11 @@ void DatabaseViewer::update(int value,
{ {
this->updateStereo(&data); this->updateStereo(&data);
} }
else
{
ui_->stereoViewer->clear();
ui_->graphicsView_stereo->clear();
}
// 3d view // 3d view
if(view3D->isVisible() && !data.getDepthRaw().empty()) 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) void DatabaseViewer::updateStereo(const Signature * data)
{ {
if(data && ui_->dockWidget_stereoView->isVisible() && !data->getImageRaw().empty() && !data->getDepthRaw().empty() && data->getDepthRaw().type() == CV_8UC1) 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; std::vector<cv::KeyPoint> kpts;
cv::Rect roi = Feature2D::computeRoi(leftMono, "0.03 0.03 0.04 0.04"); cv::Rect roi = Feature2D::computeRoi(leftMono, "0.03 0.03 0.04 0.04");
ParametersMap parameters; ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "1000")); parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0"));
parameters.insert(ParametersPair(Parameters::kGFTTMinDistance(), "5")); 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::Type type = Feature2D::kFeatureGfttBrief;
Feature2D * kptDetector = Feature2D::create(type, parameters); Feature2D * kptDetector = Feature2D::create(type, parameters);
kpts = kptDetector->generateKeypoints(leftMono, roi); kpts = kptDetector->generateKeypoints(leftMono, roi);
@@ -1720,6 +1789,19 @@ void DatabaseViewer::updateStereo(const Signature * data)
std::vector<cv::Point2f> leftCorners; std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(kpts, 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 // Find features in the new left image
std::vector<unsigned char> status; std::vector<unsigned char> status;
std::vector<float> err; std::vector<float> err;
@@ -1731,8 +1813,8 @@ void DatabaseViewer::updateStereo(const Signature * data)
rightCorners, rightCorners,
status, status,
err, err,
cv::Size(Parameters::defaultStereoWinSize(), Parameters::defaultStereoWinSize()), Parameters::defaultStereoMaxLevel(), 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, Parameters::defaultStereoIterations(), Parameters::defaultStereoEps())); cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, ui_->spinBox_stereo_flowIterations->value(), ui_->doubleSpinBox_stereo_flowEps->value()));
float timeFlow = timer.ticks(); float timeFlow = timer.ticks();
@@ -1741,6 +1823,10 @@ void DatabaseViewer::updateStereo(const Signature * data)
float bad_point = std::numeric_limits<float>::quiet_NaN (); float bad_point = std::numeric_limits<float>::quiet_NaN ();
UASSERT(status.size() == kpts.size()); UASSERT(status.size() == kpts.size());
int oi = 0; int oi = 0;
int inliers = 0;
int flowOutliers= 0;
int slopeOutliers= 0;
int negativeDisparityOutliers = 0;
for(unsigned int i=0; i<status.size(); ++i) for(unsigned int i=0; i<status.size(); ++i)
{ {
pcl::PointXYZ pt(bad_point, bad_point, bad_point); 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; float disparity = leftCorners[i].x - rightCorners[i].x;
if(disparity > 0.0f) 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( pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
leftCorners[i], leftCorners[i],
@@ -1759,23 +1845,32 @@ void DatabaseViewer::updateStereo(const Signature * data)
if(pcl::isFinite(tmpPt)) if(pcl::isFinite(tmpPt))
{ {
pt = pcl::transformPoint(tmpPt, data->getLocalTransform().toEigen3f()); pt = pcl::transformPoint(tmpPt, data->getLocalTransform().toEigen3f());
if(fabs(pt.x) > 2 || fabs(pt.y) > 2 || fabs(pt.z) > 2) status[i] = 100; //blue
{ ++inliers;
status[i] = 100; //blue
}
cloud->at(oi++) = pt; cloud->at(oi++) = pt;
} }
} }
else if(fabs(leftCorners[i].y-rightCorners[i].y) <=1.0f)
{
status[i] = 110; //cyan
++inliers;
}
else else
{ {
status[i] = 101; //yellow status[i] = 101; //yellow
++slopeOutliers;
} }
} }
else else
{ {
status[i] = 102; //magenta status[i] = 102; //magenta
++negativeDisparityOutliers;
} }
} }
else
{
++flowOutliers;
}
} }
cloud->resize(oi); cloud->resize(oi);
@@ -1786,6 +1881,11 @@ void DatabaseViewer::updateStereo(const Signature * data)
ui_->stereoViewer->addOrUpdateCloud("stereo", cloud); ui_->stereoViewer->addOrUpdateCloud("stereo", cloud);
ui_->stereoViewer->update(); 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; std::vector<cv::KeyPoint> rightKpts;
cv::KeyPoint::convert(rightCorners, rightKpts); cv::KeyPoint::convert(rightCorners, rightKpts);
std::vector<cv::DMatch> good_matches(kpts.size()); std::vector<cv::DMatch> good_matches(kpts.size());
@@ -1832,6 +1932,10 @@ void DatabaseViewer::updateStereo(const Signature * data)
{ {
c = Qt::magenta; c = Qt::magenta;
} }
else if(status[i] == 110)
{
c = Qt::cyan;
}
ui_->graphicsView_stereo->addLine( ui_->graphicsView_stereo->addLine(
kpts[i].pt.x, kpts[i].pt.x,
kpts[i].pt.y, 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 //draw lines
UASSERT(info.refCorners.size() == info.newCorners.size()); 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()) if(_ui->imageView_odometry->isLinesShown())
{ {
_ui->imageView_odometry->addLine( _ui->imageView_odometry->addLine(
info.refCorners[info.cornerInliers[i]].x, info.refCorners[i].x,
info.refCorners[info.cornerInliers[i]].y, info.refCorners[i].y,
info.newCorners[info.cornerInliers[i]].x, info.newCorners[i].x,
info.newCorners[info.cornerInliers[i]].y, info.newCorners[i].y,
Qt::blue); 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/Interval/ms", (float)data.id(), info.interval*1000.f);
_ui->statsToolBox->updateStat("Odometry/Speed/kph", (float)data.id(), x/info.interval*3.6f); _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); _ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)data.id(), time.elapsed()*1000.0);
_processingOdometry = false; _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)); 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_refId->setText(QString("New ID = %1 [%2]").arg(stat.refImageId()).arg(refMapId));
_ui->label_matchId->clear();
if(stat.extended()) 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_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)); _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()); float elapsedTime = static_cast<float>(totalTime.elapsed());
UINFO("Updating GUI time = %fs", elapsedTime/1000.0f); UINFO("Updating GUI time = %fs", elapsedTime/1000.0f);
_ui->statsToolBox->updateStat("/Gui refresh stats/ms", stat.refImageId(), elapsedTime); _ui->statsToolBox->updateStat("/Gui refresh stats/ms", stat.refImageId(), elapsedTime);
@@ -2902,7 +2910,14 @@ void MainWindow::startDetection()
if(_dataRecorder) if(_dataRecorder)
{ {
UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent"); if(_camera)
{
UEventsManager::createPipe(_camera, _dataRecorder, "CameraEvent");
}
else if(_dbReader)
{
UEventsManager::createPipe(_dbReader, _dataRecorder, "CameraEvent");
}
} }
_lastOdomPose.setNull(); _lastOdomPose.setNull();

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

@@ -50,7 +50,7 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>172</width> <width>196</width>
<height>184</height> <height>184</height>
</rect> </rect>
</property> </property>
@@ -200,6 +200,9 @@
</item> </item>
<item> <item>
<widget class="QSlider" name="horizontalSlider_A"> <widget class="QSlider" name="horizontalSlider_A">
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
</property>
<property name="orientation"> <property name="orientation">
<enum>Qt::Horizontal</enum> <enum>Qt::Horizontal</enum>
</property> </property>
@@ -233,7 +236,7 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>172</width> <width>195</width>
<height>184</height> <height>184</height>
</rect> </rect>
</property> </property>
@@ -383,6 +386,9 @@
</item> </item>
<item> <item>
<widget class="QSlider" name="horizontalSlider_B"> <widget class="QSlider" name="horizontalSlider_B">
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
</property>
<property name="orientation"> <property name="orientation">
<enum>Qt::Horizontal</enum> <enum>Qt::Horizontal</enum>
</property> </property>
@@ -479,6 +485,9 @@
</item> </item>
<item row="0" column="1"> <item row="0" column="1">
<widget class="QSlider" name="horizontalSlider_neighbors"> <widget class="QSlider" name="horizontalSlider_neighbors">
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
</property>
<property name="orientation"> <property name="orientation">
<enum>Qt::Horizontal</enum> <enum>Qt::Horizontal</enum>
</property> </property>
@@ -496,6 +505,9 @@
</item> </item>
<item row="1" column="1"> <item row="1" column="1">
<widget class="QSlider" name="horizontalSlider_loops"> <widget class="QSlider" name="horizontalSlider_loops">
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
</property>
<property name="orientation"> <property name="orientation">
<enum>Qt::Horizontal</enum> <enum>Qt::Horizontal</enum>
</property> </property>
@@ -654,6 +666,9 @@
</item> </item>
<item> <item>
<widget class="QSlider" name="horizontalSlider_iterations"> <widget class="QSlider" name="horizontalSlider_iterations">
<property name="focusPolicy">
<enum>Qt::ClickFocus</enum>
</property>
<property name="orientation"> <property name="orientation">
<enum>Qt::Horizontal</enum> <enum>Qt::Horizontal</enum>
</property> </property>
@@ -783,14 +798,14 @@
<item> <item>
<widget class="QToolBox" name="toolBox"> <widget class="QToolBox" name="toolBox">
<property name="currentIndex"> <property name="currentIndex">
<number>1</number> <number>5</number>
</property> </property>
<widget class="QWidget" name="page"> <widget class="QWidget" name="page">
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>315</width> <width>312</width>
<height>314</height> <height>314</height>
</rect> </rect>
</property> </property>
@@ -1009,7 +1024,7 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>-12</y> <y>0</y>
<width>351</width> <width>351</width>
<height>355</height> <height>355</height>
</rect> </rect>
@@ -1510,7 +1525,7 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>315</width> <width>248</width>
<height>319</height> <height>319</height>
</rect> </rect>
</property> </property>
@@ -1704,8 +1719,8 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>330</width> <width>201</width>
<height>186</height> <height>126</height>
</rect> </rect>
</property> </property>
<attribute name="label"> <attribute name="label">
@@ -1799,6 +1814,217 @@
</item> </item>
</layout> </layout>
</widget> </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> </widget>
</item> </item>
</layout> </layout>
@@ -1812,12 +2038,110 @@
<number>4</number> <number>4</number>
</attribute> </attribute>
<widget class="QWidget" name="dockWidgetContents_5"> <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> <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>
<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> </item>
</layout> </layout>
</widget> </widget>

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

@@ -63,7 +63,7 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>-613</y> <y>-450</y>
<width>755</width> <width>755</width>
<height>1591</height> <height>1591</height>
</rect> </rect>
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>3</number> <number>23</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29"> <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> <string> m</string>
</property> </property>
<property name="decimals"> <property name="decimals">
<number>2</number> <number>0</number>
</property> </property>
<property name="minimum"> <property name="minimum">
<double>0.000000000000000</double> <double>0.000000000000000</double>
</property> </property>
<property name="maximum">
<double>999.000000000000000</double>
</property>
<property name="singleStep"> <property name="singleStep">
<double>1.000000000000000</double> <double>1.000000000000000</double>
</property> </property>