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

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],