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

Added Monocular SLAM (experimental), Odometry classes refactoring

Browse Source
This commit is contained in:
Mathieu Labbe
2015-04-23 22:01:41 -04:00
parent 39dce825d6
commit fa3a2421f6
39 changed files with 4138 additions and 2702 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
corelib/src/CMakeLists.txt
View File

@@ -26,11 +26,17 @@ SET(SRC_FILES
Transform.cpp
util3d.cpp
Odometry.cpp
SensorData.cpp
Graph.cpp
Compression.cpp
Odometry.cpp
OdometryThread.cpp
OdometryBOW.cpp
OdometryOpticalFlow.cpp
OdometryMono.cpp
OdometryICP.cpp
toro3d/posegraph3.cpp
toro3d/treeoptimizer3_iteration.cpp
toro3d/treeoptimizer3.cpp

21
corelib/src/CameraRGBD.cpp
View File

@@ -77,6 +77,7 @@ CameraRGBD::CameraRGBD(float imageRate, const Transform & localTransform) :
_imageRate(imageRate),
_localTransform(localTransform),
_mirroring(false),
_colorOnly(false),
_frameRateTimer(new UTimer())
{
}
@@ -120,13 +121,23 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & f
UTimer timer;
this->captureImage(rgb, depth, fx, fy, cx, cy);
if(!rgb.empty() && !depth.empty() && _mirroring)
if(_colorOnly)
{
cv::flip(rgb,rgb,1);
cv::flip(depth,depth,1);
if(cx != 0.0f)
depth = cv::Mat();
}
if(_mirroring)
{
if(!rgb.empty())
{
cx = float(rgb.cols) - cx;
cv::flip(rgb,rgb,1);
if(cx != 0.0f)
{
cx = float(rgb.cols) - cx;
}
}
if(!depth.empty())
{
cv::flip(depth,depth,1);
}
}
if(warnFrameRateTooHigh)

8
corelib/src/DBDriver.cpp
View File

@@ -432,10 +432,10 @@ void DBDriver::getNodeData(
imageCompressed = s->getImageCompressed();
depthCompressed = s->getDepthCompressed();
laserScanCompressed = s->getLaserScanCompressed();
fx = s->getDepthFx();
fy = s->getDepthFy();
cx = s->getDepthCx();
cy = s->getDepthCy();
fx = s->getFx();
fy = s->getFy();
cx = s->getCx();
cy = s->getCy();
localTransform = s->getLocalTransform();
found = true;
}

2
corelib/src/DBDriverSqlite3.cpp
View File

@@ -2045,7 +2045,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
//metric
if(!(*i)->getDepthCompressed().empty() || !(*i)->getLaserScanCompressed().empty())
{
stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getDepthFx(), (*i)->getDepthFy(), (*i)->getDepthCx(), (*i)->getDepthCy(), (*i)->getLocalTransform());
stepDepth(ppStmt, (*i)->id(), (*i)->getDepthCompressed(), (*i)->getLaserScanCompressed(), (*i)->getFx(), (*i)->getFy(), (*i)->getCx(), (*i)->getCy(), (*i)->getLocalTransform());
}
}
// Finalize (delete) the statement

11
corelib/src/EpipolarGeometry.cpp
View File

@@ -151,13 +151,18 @@ int inFrontOfBothCameras(const cv::Mat & x, const cv::Mat & xp, const cv::Mat &
p.at<double>(2,3) = T.at<double>(2,0);
cv::Mat pts4D;
//std::vector<double> reprojErrors;
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
//EpipolarGeometry::triangulatePoints(x, xp, p0, p, cloud, reprojErrors);
cv::triangulatePoints(p0, p, x, xp, pts4D);
//http://en.wikipedia.org/wiki/Essential_matrix#3D_points_from_corresponding_image_points
int nValid = 0;
for(int i=0; i<x.cols; ++i)
{
// the five to ignore when all points are super close to the camera
if(pts4D.at<double>(2,i)/pts4D.at<double>(3,i) > 5)
//if(cloud->at(i).z > 5)
{
++nValid;
}
@@ -210,7 +215,7 @@ cv::Mat EpipolarGeometry::findPFromE(const cv::Mat & E,
cv::Mat r = u*w*vt;
if(cv::determinant(r)+1.0 < 1e-09) {
//according to http://en.wikipedia.org/wiki/Essential_matrix#Showing_that_it_is_valid
UWARN("det(R) == -1 [%f]: flip E's sign", cv::determinant(r));
UDEBUG("det(R) == -1 [%f]: flip E's sign", cv::determinant(r));
e = -E;
svd(e,cv::SVD::MODIFY_A);
u = svd.u;
@@ -373,14 +378,10 @@ void EpipolarGeometry::findRTFromP(
cv::Mat & t)
{
UASSERT(p.cols == 4 && p.rows == 3);
UDEBUG("");
r = cv::Mat(p, cv::Range(0,3), cv::Range(0,3));
UDEBUG("");
//r = -r.inv();
UDEBUG("r=%d %d, t=%d", r.cols, r.rows, p.col(3).rows);
//t = r*p.col(3);
t = p.col(3);
UDEBUG("");
}
cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy, double cx, double cy, double Tx, double Ty)

21
corelib/src/Features2d.cpp
View File

@@ -319,6 +319,14 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> &
/////////////////////
// Feature2D
/////////////////////
Feature2D::Feature2D(const ParametersMap & parameters) :
maxFeatures_(Parameters::defaultKpWordsPerImage())
{
}
void Feature2D::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), maxFeatures_);
}
Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & parameters)
{
if(RTABMAP_NONFREE == 0 &&
@@ -369,9 +377,8 @@ Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & para
}
return feature2D;
}
std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, int maxKeypoints, const cv::Rect & roi) const
std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
{
ULOGGER_DEBUG("");
std::vector<cv::KeyPoint> keypoints;
if(!image.empty() && image.channels() == 1 && image.type() == CV_8U)
{
@@ -381,7 +388,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, in
keypoints = this->generateKeypointsImpl(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows));
ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
limitKeypoints(keypoints, maxKeypoints);
limitKeypoints(keypoints, maxFeatures_);
if(roi.x || roi.y)
{
@@ -615,7 +622,7 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
//ORB
//////////////////////////
ORB::ORB(const ParametersMap & parameters) :
nFeatures_(Parameters::defaultORBNFeatures()),
nFeatures_(Parameters::defaultKpWordsPerImage()),
scaleFactor_(Parameters::defaultORBScaleFactor()),
nLevels_(Parameters::defaultORBNLevels()),
edgeThreshold_(Parameters::defaultORBEdgeThreshold()),
@@ -646,7 +653,7 @@ ORB::~ORB()
void ORB::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kORBNFeatures(), nFeatures_);
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), nFeatures_);
Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor_);
Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels_);
Parameters::parse(parameters, Parameters::kORBEdgeThreshold(), edgeThreshold_);
@@ -903,7 +910,7 @@ cv::Mat FAST_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<c
//GFTT
//////////////////////////
GFTT::GFTT(const ParametersMap & parameters) :
_maxCorners(Parameters::defaultGFTTMaxCorners()),
_maxCorners(Parameters::defaultKpWordsPerImage()),
_qualityLevel(Parameters::defaultGFTTQualityLevel()),
_minDistance(Parameters::defaultGFTTMinDistance()),
_blockSize(Parameters::defaultGFTTBlockSize()),
@@ -924,7 +931,7 @@ GFTT::~GFTT()
void GFTT::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kGFTTMaxCorners(), _maxCorners);
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), _maxCorners);
Parameters::parse(parameters, Parameters::kGFTTQualityLevel(), _qualityLevel);
Parameters::parse(parameters, Parameters::kGFTTMinDistance(), _minDistance);
Parameters::parse(parameters, Parameters::kGFTTBlockSize(), _blockSize);

325
corelib/src/Memory.cpp
View File

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Parameters.h"
#include "rtabmap/core/RtabmapEvent.h"
#include "rtabmap/core/VWDictionary.h"
#include <rtabmap/core/EpipolarGeometry.h>
#include "VisualWord.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/util3d.h"
@@ -85,7 +86,6 @@ Memory::Memory(const ParametersMap & parameters) :
_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
_parallelized(Parameters::defaultKpParallelized()),
_wordsMaxDepth(Parameters::defaultKpMaxDepth()),
_wordsPerImageTarget(Parameters::defaultKpWordsPerImage()),
_roiRatios(std::vector<float>(4, 0.0f)),
_bowMinInliers(Parameters::defaultLccBowMinInliers()),
@@ -93,6 +93,8 @@ Memory::Memory(const ParametersMap & parameters) :
_bowIterations(Parameters::defaultLccBowIterations()),
_bowMaxDepth(Parameters::defaultLccBowMaxDepth()),
_bowForce2D(Parameters::defaultLccBowForce2D()),
_bowEpipolarGeometry(Parameters::defaultLccBowEpipolarGeometry()),
_bowEpipolarGeometryVar(Parameters::defaultLccBowEpipolarGeometryVar()),
_icpDecimation(Parameters::defaultLccIcp3Decimation()),
_icpMaxDepth(Parameters::defaultLccIcp3MaxDepth()),
@@ -420,6 +422,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kLccBowIterations(), _bowIterations);
Parameters::parse(parameters, Parameters::kLccBowMaxDepth(), _bowMaxDepth);
Parameters::parse(parameters, Parameters::kLccBowForce2D(), _bowForce2D);
Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometry(), _bowEpipolarGeometry);
Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometryVar(), _bowEpipolarGeometryVar);
Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), _icpDecimation);
Parameters::parse(parameters, Parameters::kLccIcp3MaxDepth(), _icpMaxDepth);
Parameters::parse(parameters, Parameters::kLccIcp3VoxelSize(), _icpVoxelSize);
@@ -468,7 +472,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kKpParallelized(), _parallelized);
Parameters::parse(parameters, Parameters::kKpBadSignRatio(), _badSignRatio);
Parameters::parse(parameters, Parameters::kKpMaxDepth(), _wordsMaxDepth);
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), _wordsPerImageTarget);
Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
@@ -1142,7 +1145,6 @@ std::map<int, float> Memory::computeLikelihood(const Signature * signature, cons
}
else
{
// TODO cleanup , old way...
UTimer timer;
timer.start();
std::map<int, float> likelihood;
@@ -1834,81 +1836,156 @@ Transform Memory::computeVisualTransform(
const Signature & newS,
std::string * rejectedMsg,
int * inliers,
double * variance) const
double * varianceOut) const
{
Transform transform;
std::string msg;
// Guess transform from visual words
if(!oldS.getWords3().empty() && !newS.getWords3().empty())
if(_bowEpipolarGeometry)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersOld(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersNew(new pcl::PointCloud<pcl::PointXYZ>);
util3d::findCorrespondences(
oldS.getWords3(),
newS.getWords3(),
*inliersOld,
*inliersNew,
_bowMaxDepth);
if((int)inliersOld->size() >= _bowMinInliers)
// we only need the camera transform, send guess words3 for scale estimation
if(oldS.getWords3().size())
{
UDEBUG("Correspondences = %d", (int)inliersOld->size());
int inliersCount = 0;
std::vector<int> inliersV;
Transform t = util3d::transformFromXYZCorrespondences(
inliersOld,
inliersNew,
_bowInlierDistance,
_bowIterations,
true, 3.0, 10,
&inliersV,
variance);
inliersCount = (int)inliersV.size();
if(!t.isNull() && inliersCount >= _bowMinInliers)
Transform cameraTransform;
double variance = 1;
std::multimap<int, pcl::PointXYZ> inliers3D = util3d::generateWords3DMono(
oldS.getWords(),
newS.getWords(),
oldS.getFx(),
oldS.getFy(),
oldS.getCx(),
oldS.getCy(),
oldS.getLocalTransform(),
cameraTransform,
100,
4.0f,
cv::ITERATIVE,
1.0f,
0.99f,
oldS.getWords3(),
&variance);
if(varianceOut)
{
transform = t;
if(_bowForce2D)
{
UDEBUG("Forcing 2D...");
float x,y,z,r,p,yaw;
transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
transform = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
}
*varianceOut = variance;
}
else if(inliersCount < _bowMinInliers)
{
msg = uFormat("Not enough inliers (after RANSAC) %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id());
UINFO(msg.c_str());
}
else if(inliersCount == (int)inliersOld->size())
{
msg = uFormat("Rejected identity with full inliers.");
UINFO(msg.c_str());
}
if(inliers)
{
*inliers = inliersCount;
*inliers = inliers3D.size();
}
if(!cameraTransform.isNull())
{
if((int)inliers3D.size() >= _bowMinInliers)
{
if(variance <= _bowEpipolarGeometryVar)
{
transform = cameraTransform.inverse();
}
else
{
msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _bowEpipolarGeometryVar, variance);
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _bowMinInliers);
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("No camera transform found");
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough inliers %d/%d between %d and %d", (int)inliersOld->size(), _bowMinInliers, oldS.id(), newS.id());
UINFO(msg.c_str());
msg = uFormat("No 3D guess words found");
UWARN(msg.c_str());
}
}
else if(!oldS.isBadSignature() && !newS.isBadSignature())
else
{
msg = "Words 3D empty?!?";
UERROR(msg.c_str());
if(!oldS.getWords3().empty() && !newS.getWords3().empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersOld(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersNew(new pcl::PointCloud<pcl::PointXYZ>);
util3d::findCorrespondences(
oldS.getWords3(),
newS.getWords3(),
*inliersOld,
*inliersNew,
_bowMaxDepth);
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs2d;
EpipolarGeometry::findPairsUnique(oldS.getWords(), newS.getWords(), pairs2d);
UDEBUG("3D unique Correspondences = %d (2D unique pairs=%d) words=%d and %d",
(int)inliersOld->size(), (int)pairs2d.size(), (int)oldS.getWords3().size(), (int)newS.getWords3().size());
if((int)inliersOld->size() >= _bowMinInliers)
{
int inliersCount = 0;
std::vector<int> inliersV;
Transform t = util3d::transformFromXYZCorrespondences(
inliersOld,
inliersNew,
_bowInlierDistance,
_bowIterations,
true, 3.0, 10,
&inliersV,
varianceOut);
inliersCount = (int)inliersV.size();
if(!t.isNull() && inliersCount >= _bowMinInliers)
{
transform = t;
if(_bowForce2D)
{
UDEBUG("Forcing 2D...");
float x,y,z,r,p,yaw;
transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
transform = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
}
}
else if(inliersCount < _bowMinInliers)
{
msg = uFormat("Not enough inliers (after RANSAC) %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id());
UINFO(msg.c_str());
}
else if(inliersCount == (int)inliersOld->size())
{
msg = uFormat("Rejected identity with full inliers.");
UINFO(msg.c_str());
}
if(inliers)
{
*inliers = inliersCount;
}
}
else
{
msg = uFormat("Not enough inliers %d/%d between %d and %d", (int)inliersOld->size(), _bowMinInliers, oldS.id(), newS.id());
UINFO(msg.c_str());
}
}
else if(!oldS.isBadSignature() && !newS.isBadSignature() && (oldS.getWords3().size()==0 || newS.getWords3().size()==0))
{
msg = uFormat("Words 3D empty?!? olds=%d=%d newS=%d=%d",
oldS.id(), (int)oldS.getWords3().size(),
newS.id(), (int)newS.getWords3().size());
UWARN(msg.c_str());
}
}
if(rejectedMsg)
{
*rejectedMsg = msg;
}
UDEBUG("transform=%s", transform.prettyPrint().c_str());
return transform;
}
@@ -2031,10 +2108,10 @@ Transform Memory::computeIcpTransform(
{
pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloudXYZ = util3d::getICPReadyCloud(
oldS.getDepthRaw(),
oldS.getDepthFx(),
oldS.getDepthFy(),
oldS.getDepthCx(),
oldS.getDepthCy(),
oldS.getFx(),
oldS.getFy(),
oldS.getCx(),
oldS.getCy(),
_icpDecimation,
_icpMaxDepth,
_icpVoxelSize,
@@ -2042,10 +2119,10 @@ Transform Memory::computeIcpTransform(
oldS.getLocalTransform());
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
newS.getDepthRaw(),
newS.getDepthFx(),
newS.getDepthFy(),
newS.getDepthCx(),
newS.getDepthCy(),
newS.getFx(),
newS.getFy(),
newS.getCx(),
newS.getCy(),
_icpDecimation,
_icpMaxDepth,
_icpVoxelSize,
@@ -3188,7 +3265,7 @@ void Memory::copyData(const Signature * from, Signature * to)
else
{
to->setImageCompressed(from->getImageCompressed());
to->setDepthCompressed(from->getDepthCompressed(), from->getDepthFx(), from->getDepthFy(), from->getDepthCx(), from->getDepthCy());
to->setDepthCompressed(from->getDepthCompressed(), from->getFx(), from->getFy(), from->getCx(), from->getCy());
to->setLaserScanCompressed(from->getLaserScanCompressed());
to->setLocalTransform(from->getLocalTransform());
}
@@ -3221,6 +3298,7 @@ private:
Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
{
UDEBUG("");
UASSERT(data.image().empty() || data.image().type() == CV_8UC1 || data.image().type() == CV_8UC3);
UASSERT(data.depth().empty() || ((data.depth().type() == CV_16UC1 || data.depth().type() == CV_32FC1) && data.depth().rows == data.image().rows && data.depth().cols == data.image().cols));
UASSERT(data.rightImage().empty() || (data.rightImage().type() == CV_8UC1 && data.rightImage().rows == data.image().rows && data.rightImage().cols == data.image().cols));
@@ -3289,7 +3367,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3D(new pcl::PointCloud<pcl::PointXYZ>);
if(data.keypoints().size() == 0)
{
if(_wordsPerImageTarget >= 0)
if(_feature2D->getMaxFeatures() >= 0)
{
// Extract features
cv::Mat imageMono;
@@ -3313,7 +3391,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
{
subPixelOn = true;
}
keypoints = _feature2D->generateKeypoints(imageMono, subPixelOn?_wordsPerImageTarget:0, roi);
keypoints = _feature2D->generateKeypoints(imageMono, roi);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
@@ -3341,7 +3419,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemStereo_subpixel(), t*1000.0f);
if(stats) stats->addStatistic(Statistics::kTimingMemSubpixel(), t*1000.0f);
UDEBUG("time subpix left kpts=%fs", t);
}
else
@@ -3371,16 +3449,6 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
UDEBUG("filter keypoints by disparity (%d)", (int)keypoints.size());
}
if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
{
Feature2D::limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
UDEBUG("limit keypoints max (%d)", _wordsPerImageTarget);
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_filtering(), t*1000.0f);
UDEBUG("time keypoints filtering = %fs", _wordsPerImageTarget);
if(keypoints.size())
{
if(!subPixelOn)
@@ -3406,7 +3474,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
{
subPixelOn = true;
}
keypoints = _feature2D->generateKeypoints(imageMono, subPixelOn?_wordsPerImageTarget:0, roi);
keypoints = _feature2D->generateKeypoints(imageMono, roi);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
@@ -3434,7 +3502,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemStereo_subpixel(), t*1000.0f);
if(stats) stats->addStatistic(Statistics::kTimingMemSubpixel(), t*1000.0f);
UDEBUG("time subpix left kpts=%fs", t);
}
@@ -3444,15 +3512,6 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
UDEBUG("filter keypoints by depth (%d)", (int)keypoints.size());
}
if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
{
Feature2D::limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
UDEBUG("limit keypoints max (%d)", _wordsPerImageTarget);
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_filtering(), t*1000.0f);
UDEBUG("time keypoints filtering = %fs", _wordsPerImageTarget);
if(keypoints.size())
{
if(!subPixelOn)
@@ -3473,7 +3532,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
else
{
//RGB only
keypoints = _feature2D->generateKeypoints(imageMono, _wordsPerImageTarget, roi);
keypoints = _feature2D->generateKeypoints(imageMono, roi);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
@@ -3484,6 +3543,25 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
if(_subPixWinSize > 0 && _subPixIterations > 0)
{
std::vector<cv::Point2f> corners;
cv::KeyPoint::convert(keypoints, corners);
cv::cornerSubPix( imageMono, corners,
cv::Size( _subPixWinSize, _subPixWinSize ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _subPixIterations, _subPixEps ) );
for(unsigned int i=0;i<corners.size(); ++i)
{
keypoints[i].pt = corners[i];
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemSubpixel(), t*1000.0f);
UDEBUG("time subpix kpts=%fs", t);
}
}
}
@@ -3495,7 +3573,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
}
else
{
UDEBUG("_wordsPerImageTarget(%d)<0 so don't extract any descriptors...", _wordsPerImageTarget);
UDEBUG("_feature2D->getMaxFeatures()(%d<0) so don't extract any features...", _feature2D->getMaxFeatures());
}
}
else
@@ -3540,15 +3618,6 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
Feature2D::filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity);
}
if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
{
Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
UDEBUG("limit keypoints max (%d)", _wordsPerImageTarget);
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_filtering(), t*1000.0f);
UDEBUG("time keypoints filtering=%fs", t);
keypoints3D = util3d::generateKeypoints3DDisparity(keypoints, disparity, data.fx(), data.baseline(), data.cx(), data.cy(), data.localTransform());
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
@@ -3563,28 +3632,11 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
UDEBUG("filter keypoints by depth (%d)", (int)keypoints.size());
}
if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
{
Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
UDEBUG("limit keypoints max (%d)", _wordsPerImageTarget);
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_filtering(), t*1000.0f);
UDEBUG("time keypoints filtering=%fs", t);
keypoints3D = util3d::generateKeypoints3DDepth(keypoints, data.depth(), data.fx(), data.fy(), data.cx(), data.cy(), data.localTransform());
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
}
else
{
// RGB only
Feature2D::limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_filtering(), t*1000.0f);
UDEBUG("time keypoints filtering=%fs", t);
}
}
if(_parallelized)
@@ -3644,6 +3696,47 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
}
}
if(words.size() > 8 &&
words3D.size() == 0 &&
!data.pose().isNull() &&
_signatures.size())
{
UDEBUG("Generate 3D words using odometry");
Signature * previousS = _signatures.rbegin()->second;
if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
{
Transform cameraTransform = data.pose().inverse() * previousS->getPose();
// compute 3D words by epipolar geometry with the previous signature
std::multimap<int, pcl::PointXYZ> inliers = util3d::generateWords3DMono(
words,
previousS->getWords(),
data.fx(), data.fy()?data.fy():data.fx(),
data.cx(), data.cy(),
data.localTransform(),
cameraTransform);
// words3D should have the same size than words
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
std::multimap<int, pcl::PointXYZ>::iterator jter=inliers.find(iter->first);
if(jter != inliers.end())
{
words3D.insert(std::make_pair(iter->first, jter->second));
}
else
{
words3D.insert(std::make_pair(iter->first, pcl::PointXYZ(bad_point,bad_point,bad_point)));
}
}
t = timer.ticks();
UASSERT(words3D.size() == words.size());
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
}
}
cv::Mat image = data.image();
cv::Mat depthOrRightImage = data.depthOrRightImage();
float fx = data.fx();

1602
corelib/src/Odometry.cpp
View File

File diff suppressed because it is too large Load Diff

463
corelib/src/OdometryBOW.cpp Normal file
View File

@@ -0,0 +1,463 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Odometry.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
#include <opencv2/calib3d/calib3d.hpp>
#if _MSC_VER
#define ISFINITE(value) _finite(value)
#else
#define ISFINITE(value) std::isfinite(value)
#endif
namespace rtabmap {
OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
Odometry(parameters),
_localHistoryMaxSize(Parameters::defaultOdomBowLocalHistorySize()),
_memory(0)
{
Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), _localHistoryMaxSize);
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), this->getRoiRatios()));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
int nn = Parameters::defaultOdomBowNNType();
float nndr = Parameters::defaultOdomBowNNDR();
int featureType = Parameters::defaultOdomFeatureType();
int maxFeatures = Parameters::defaultOdomMaxFeatures();
Parameters::parse(parameters, Parameters::kOdomBowNNType(), nn);
Parameters::parse(parameters, Parameters::kOdomBowNNDR(), nndr);
Parameters::parse(parameters, Parameters::kOdomFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
// Memory's stereo parameters, copy from Odometry
int subPixWinSize = Parameters::defaultOdomSubPixWinSize();
int subPixIterations = Parameters::defaultOdomSubPixIterations();
double subPixEps = Parameters::defaultOdomSubPixEps();
Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize);
Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations);
Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps);
customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));
// add only feature stuff
for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string group = uSplit(iter->first, '/').front();
if(group.compare("SURF") == 0 ||
group.compare("SIFT") == 0 ||
group.compare("BRIEF") == 0 ||
group.compare("FAST") == 0 ||
group.compare("ORB") == 0 ||
group.compare("FREAK") == 0 ||
group.compare("GFTT") == 0 ||
group.compare("BRISK") == 0)
{
customParameters.insert(*iter);
}
}
_memory = new Memory(customParameters);
if(!_memory->init("", false, ParametersMap()))
{
UERROR("Error initializing the memory for BOW Odometry.");
}
}
OdometryBOW::~OdometryBOW()
{
delete _memory;
UDEBUG("");
}
void OdometryBOW::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
_memory->init("", false, ParametersMap());
localMap_.clear();
}
// return not null transform if odometry is correctly computed
Transform OdometryBOW::computeTransform(
const SensorData & data,
OdometryInfo * info)
{
UTimer timer;
Transform output;
if(info)
{
info->type = 0;
}
double variance = 0;
int inliers = 0;
int correspondences = 0;
int nFeatures = 0;
const Signature * previousSignature = _memory->getLastWorkingSignature();
if(_memory->update(data))
{
const Signature * newSignature = _memory->getLastWorkingSignature();
if(newSignature)
{
nFeatures = (int)newSignature->getWords().size();
if(this->isInfoDataFilled() && info)
{
info->words = newSignature->getWords();
}
}
if(previousSignature && newSignature)
{
Transform transform;
if((int)localMap_.size() >= this->getMinInliers())
{
if(this->isPnPEstimationUsed())
{
if((int)newSignature->getWords().size() >= this->getMinInliers())
{
// find correspondences
std::vector<int> ids = uListToVector(uUniqueKeys(newSignature->getWords()));
std::vector<cv::Point3f> objectPoints(ids.size());
std::vector<cv::Point2f> imagePoints(ids.size());
int oi=0;
std::vector<int> matches(ids.size());
for(unsigned int i=0; i<ids.size(); ++i)
{
if(localMap_.count(ids[i]) == 1)
{
pcl::PointXYZ pt = localMap_.find(ids[i])->second;
objectPoints[oi].x = pt.x;
objectPoints[oi].y = pt.y;
objectPoints[oi].z = pt.z;
imagePoints[oi] = newSignature->getWords().find(ids[i])->second.pt;
matches[oi++] = ids[i];
}
}
objectPoints.resize(oi);
imagePoints.resize(oi);
matches.resize(oi);
if(this->isInfoDataFilled() && info)
{
info->wordMatches.insert(info->wordMatches.end(), matches.begin(), matches.end());
}
correspondences = (int)matches.size();
if((int)matches.size() >= this->getMinInliers())
{
//PnPRansac
cv::Mat K = (cv::Mat_<double>(3,3) <<
data.fx(), 0, data.cx(),
0, data.fy()>0?data.fy():data.fx(), data.cy(),
0, 0, 1);
Transform guess = (this->getPose() * 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
transform = (data.localTransform() * pnp * this->getPose()).inverse();
UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
// compute variance (like in PCL computeVariance() method of sac_model.h)
std::vector<float> errorSqrdDists(inliersV.size());
for(unsigned int i=0; i<inliersV.size(); ++i)
{
std::multimap<int, pcl::PointXYZ>::const_iterator iter = newSignature->getWords3().find(matches[inliersV[i]]);
UASSERT(iter != newSignature->getWords3().end());
const cv::Point3f & objPt = objectPoints[inliersV[i]];
pcl::PointXYZ newPt = util3d::transformPoint(iter->second, this->getPose()*transform);
errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
}
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 && inliersV.size())
{
info->wordInliers.resize(inliersV.size());
for(unsigned int i=0; i<inliersV.size(); ++i)
{
info->wordInliers[i] = matches[inliersV[i]];
}
}
}
else
{
UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
}
}
else
{
UWARN("Not enough features in the new image (%d < %d)", (int)newSignature->getWords().size(), this->getMinInliers());
}
}
else
{
if((int)newSignature->getWords3().size() >= this->getMinInliers())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
// No need to set max depth here, it is already applied in extractKeypointsAndDescriptors() above.
// Also! the localMap_ have points not in camera frame anymore (in local map frame), so filtering
// by depth here is wrong!
std::set<int> uniqueCorrespondences;
util3d::findCorrespondences(
localMap_,
newSignature->getWords3(),
*inliers1,
*inliers2,
0,
&uniqueCorrespondences);
UDEBUG("localMap=%d, new=%d, unique correspondences=%d", (int)localMap_.size(), (int)newSignature->getWords3().size(), (int)uniqueCorrespondences.size());
if(this->isInfoDataFilled() && info)
{
info->wordMatches.insert(info->wordMatches.end(), uniqueCorrespondences.begin(), uniqueCorrespondences.end());
}
correspondences = (int)inliers1->size();
if((int)inliers1->size() >= this->getMinInliers())
{
// the transform returned is global odometry pose, not incremental one
std::vector<int> inliersV;
Transform t = util3d::transformFromXYZCorrespondences(
inliers2,
inliers1,
this->getInlierDistance(),
this->getIterations(),
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
&inliersV,
&variance);
inliers = (int)inliersV.size();
if(!t.isNull() && inliers >= this->getMinInliers())
{
// make it incremental
transform = this->getPose().inverse() * t;
UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
}
else
{
UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
}
if(this->isInfoDataFilled() && info && inliersV.size())
{
info->wordInliers.resize(inliersV.size());
for(unsigned int i=0; i<inliersV.size(); ++i)
{
info->wordInliers[i] = info->wordMatches[inliersV[i]];
}
}
}
else
{
UWARN("Not enough inliers %d < %d", (int)inliers1->size(), this->getMinInliers());
}
}
else
{
UWARN("Not enough 3D features in the new image (%d < %d)", (int)newSignature->getWords3().size(), this->getMinInliers());
}
}
}
else
{
UWARN("Local map too small!? (%d < %d)", (int)localMap_.size(), this->getMinInliers());
}
if(transform.isNull())
{
_memory->deleteLocation(newSignature->id());
}
else
{
output = transform;
// remove words if history max size is reached
while(localMap_.size() && (int)localMap_.size() > _localHistoryMaxSize && _memory->getStMem().size()>1)
{
int nodeId = *_memory->getStMem().begin();
std::list<int> removedPts;
_memory->deleteLocation(nodeId, &removedPts);
for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
{
localMap_.erase(*iter);
}
}
if(_localHistoryMaxSize == 0 && localMap_.size() > 0 && localMap_.size() > newSignature->getWords3().size())
{
UERROR("Local map should have only words of the last added signature here! (size=%d, max history size=%d, newWords=%d)",
(int)localMap_.size(), _localHistoryMaxSize, (int)newSignature->getWords3().size());
}
// update local map
std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
Transform t = this->getPose()*output;
for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
{
// Only add unique words not in local map
if(newSignature->getWords3().count(*iter) == 1)
{
// keep old word
if(localMap_.find(*iter) == localMap_.end())
{
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt))
{
pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
localMap_.insert(std::make_pair(*iter, pt2));
}
}
}
else
{
localMap_.erase(*iter);
}
}
}
}
else if(!previousSignature && newSignature)
{
localMap_.clear();
int count = 0;
std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
if((int)uniques.size() >= this->getMinInliers())
{
output.setIdentity();
Transform t = this->getPose(); // initial pose maybe not identity...
for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
{
// Only add unique words
if(newSignature->getWords3().count(*iter) == 1)
{
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt))
{
pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
localMap_.insert(std::make_pair(*iter, pt2));
}
else
{
++count;
}
}
}
}
else
{
// not enough features, just delete it
_memory->deleteLocation(newSignature->id());
}
UDEBUG("uniques=%d, pt not finite = %d", (int)uniques.size(),count);
}
_memory->emptyTrash();
}
if(info)
{
info->variance = variance;
info->inliers = inliers;
info->matches = correspondences;
info->features = nFeatures;
info->localMapSize = (int)localMap_.size();
}
UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance=%f local_map=%d dict=%d nodes=%d",
timer.elapsed(),
output.isNull()?"true":"false",
nFeatures,
inliers,
correspondences,
variance,
(int)localMap_.size(),
(int)_memory->getVWDictionary()->getVisualWords().size(),
(int)_memory->getStMem().size());
return output;
}
} // namespace rtabmap

191
corelib/src/OdometryICP.cpp Normal file
View File

@@ -0,0 +1,191 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Odometry.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
namespace rtabmap {
OdometryICP::OdometryICP(int decimation,
float voxelSize,
int samples,
float maxCorrespondenceDistance,
int maxIterations,
float correspondenceRatio,
bool pointToPlane,
const ParametersMap & odometryParameter) :
Odometry(odometryParameter),
_decimation(decimation),
_voxelSize(voxelSize),
_samples(samples),
_maxCorrespondenceDistance(maxCorrespondenceDistance),
_maxIterations(maxIterations),
_correspondenceRatio(correspondenceRatio),
_pointToPlane(pointToPlane),
_previousCloudNormal(new pcl::PointCloud<pcl::PointNormal>),
_previousCloud(new pcl::PointCloud<pcl::PointXYZ>)
{
}
void OdometryICP::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
_previousCloudNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
_previousCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
}
// return not null transform if odometry is correctly computed
Transform OdometryICP::computeTransform(const SensorData & data, OdometryInfo * info)
{
UTimer timer;
Transform output;
bool hasConverged = false;
double variance = 0;
unsigned int minPoints = 100;
if(!data.depth().empty())
{
if(data.depth().type() == CV_8UC1)
{
UERROR("ICP 3D cannot be done on stereo images!");
return output;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
data.depth(),
data.fx(),
data.fy(),
data.cx(),
data.cy(),
_decimation,
this->getMaxDepth(),
_voxelSize,
_samples,
data.localTransform());
if(_pointToPlane)
{
pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
std::vector<int> indices;
newCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(newCloud);
if(newCloudXYZ->size() != newCloud->size())
{
UWARN("removed nan normals...");
}
if(_previousCloudNormal->size() > minPoints && newCloud->size() > minPoints)
{
int correspondences = 0;
Transform transform = util3d::icpPointToPlane(newCloud,
_previousCloudNormal,
_maxCorrespondenceDistance,
_maxIterations,
&hasConverged,
&variance,
&correspondences);
// verify if there are enough correspondences
float correspondencesRatio = float(correspondences)/float(_previousCloudNormal->size()>newCloud->size()?_previousCloudNormal->size():newCloud->size());
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= _correspondenceRatio)
{
output = transform;
_previousCloudNormal = newCloud;
}
else
{
UWARN("Transform not valid (hasConverged=%s variance = %f)",
hasConverged?"true":"false", variance);
}
}
else if(newCloud->size() > minPoints)
{
output.setIdentity();
_previousCloudNormal = newCloud;
}
}
else
{
//point to point
if(_previousCloud->size() > minPoints && newCloudXYZ->size() > minPoints)
{
int correspondences = 0;
Transform transform = util3d::icp(newCloudXYZ,
_previousCloud,
_maxCorrespondenceDistance,
_maxIterations,
&hasConverged,
&variance,
&correspondences);
// verify if there are enough correspondences
float correspondencesRatio = float(correspondences)/float(_previousCloud->size()>newCloudXYZ->size()?_previousCloud->size():newCloudXYZ->size());
if(!transform.isNull() && hasConverged &&
correspondencesRatio >= _correspondenceRatio)
{
output = transform;
_previousCloud = newCloudXYZ;
}
else
{
UWARN("Transform not valid (hasConverged=%s variance = %f)",
hasConverged?"true":"false", variance);
}
}
else if(newCloudXYZ->size() > minPoints)
{
output.setIdentity();
_previousCloud = newCloudXYZ;
}
}
}
else
{
UERROR("Depth is empty?!?");
}
if(info)
{
info->variance = variance;
}
UINFO("Odom update time = %fs hasConverged=%s variance=%f cloud=%d",
timer.elapsed(),
hasConverged?"true":"false",
variance,
(int)(_pointToPlane?_previousCloudNormal->size():_previousCloud->size()));
return output;
}
} // namespace rtabmap

930
corelib/src/OdometryMono.cpp Normal file
View File

@@ -0,0 +1,930 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Odometry.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UStl.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/video/tracking.hpp>
namespace rtabmap {
OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
Odometry(parameters),
flowWinSize_(Parameters::defaultOdomFlowWinSize()),
flowIterations_(Parameters::defaultOdomFlowIterations()),
flowEps_(Parameters::defaultOdomFlowEps()),
flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
localHistoryMaxSize_(Parameters::defaultOdomBowLocalHistorySize()),
initMinFlow_(Parameters::defaultOdomMonoInitMinFlow()),
initMinTranslation_(Parameters::defaultOdomMonoInitMinTranslation()),
minTranslation_(Parameters::defaultOdomMonoMinTranslation()),
fundMatrixReprojError_(Parameters::defaultVhEpRansacParam1()),
fundMatrixConfidence_(Parameters::defaultVhEpRansacParam2()),
maxVariance_(Parameters::defaultOdomMonoMaxVariance())
{
Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), localHistoryMaxSize_);
Parameters::parse(parameters, Parameters::kOdomMonoInitMinFlow(), initMinFlow_);
Parameters::parse(parameters, Parameters::kOdomMonoInitMinTranslation(), initMinTranslation_);
Parameters::parse(parameters, Parameters::kOdomMonoMinTranslation(), minTranslation_);
Parameters::parse(parameters, Parameters::kOdomMonoMaxVariance(), maxVariance_);
Parameters::parse(parameters, Parameters::kVhEpRansacParam1(), fundMatrixReprojError_);
Parameters::parse(parameters, Parameters::kVhEpRansacParam2(), fundMatrixConfidence_);
// Setup memory
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), this->getRoiRatios()));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "true"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kKpTfIdfLikelihoodUsed(), "false"));
int nn = Parameters::defaultOdomBowNNType();
float nndr = Parameters::defaultOdomBowNNDR();
int featureType = Parameters::defaultOdomFeatureType();
int maxFeatures = Parameters::defaultOdomMaxFeatures();
Parameters::parse(parameters, Parameters::kOdomBowNNType(), nn);
Parameters::parse(parameters, Parameters::kOdomBowNNDR(), nndr);
Parameters::parse(parameters, Parameters::kOdomFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
int subPixWinSize = Parameters::defaultOdomSubPixWinSize();
int subPixIterations = Parameters::defaultOdomSubPixIterations();
double subPixEps = Parameters::defaultOdomSubPixEps();
Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize);
Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations);
Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps);
customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));
// add only feature stuff
for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string group = uSplit(iter->first, '/').front();
if(group.compare("SURF") == 0 ||
group.compare("SIFT") == 0 ||
group.compare("BRIEF") == 0 ||
group.compare("FAST") == 0 ||
group.compare("ORB") == 0 ||
group.compare("FREAK") == 0 ||
group.compare("GFTT") == 0 ||
group.compare("BRISK") == 0)
{
customParameters.insert(*iter);
}
}
memory_ = new Memory(customParameters);
if(!memory_->init("", false, ParametersMap()))
{
UERROR("Error initializing the memory for Mono Odometry.");
}
}
OdometryMono::~OdometryMono()
{
delete memory_;
}
void OdometryMono::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
memory_->init("", false, ParametersMap());
localMap_.clear();
refDepth_ = cv::Mat();
cornersMap_.clear();
keyFrameWords3D_.clear();
keyFramePoses_.clear();
}
Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo * info)
{
UASSERT(!data.image().empty());
UASSERT(data.fx());
UTimer timer;
Transform output;
int inliers = 0;
int correspondences = 0;
int nFeatures = 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();
}
if(memory_->getStMem().size() >= 1)
{
if(localMap_.size())
{
//PnP
UDEBUG("PnP");
if(this->isInfoDataFilled() && info)
{
info->type = 0;
}
// generate kpts
if(memory_->update(SensorData(newFrame)))
{
UDEBUG("");
bool newPtsAdded = false;
const Signature * newS = memory_->getLastWorkingSignature();
UDEBUG("newWords=%d", (int)newS->getWords().size());
nFeatures = (int)newS->getWords().size();
if((int)newS->getWords().size() > this->getMinInliers())
{
cv::Mat K = (cv::Mat_<double>(3,3) <<
data.fx(), 0, data.cx(),
0, data.fy()==0?data.fx():data.fy(), data.cy(),
0, 0, 1);
Transform guess = (this->getPose() * 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;
std::vector<cv::Point2f> imagePoints;
std::vector<int> matches;
UDEBUG("compute PnP from optical flow");
std::vector<int> ids = uKeys(localMap_);
objectPoints = uValues(localMap_);
// compute last projection
UDEBUG("project points to previous image");
std::vector<cv::Point2f> prevImagePoints;
const Signature * prevS = memory_->getSignature(*(++memory_->getStMem().rbegin()));
Transform prevGuess = (keyFramePoses_.at(prevS->id()) * data.localTransform()).inverse();
cv::Mat prevR = (cv::Mat_<double>(3,3) <<
(double)prevGuess.r11(), (double)prevGuess.r12(), (double)prevGuess.r13(),
(double)prevGuess.r21(), (double)prevGuess.r22(), (double)prevGuess.r23(),
(double)prevGuess.r31(), (double)prevGuess.r32(), (double)prevGuess.r33());
cv::Mat prevRvec(1,3, CV_64FC1);
cv::Rodrigues(prevR, prevRvec);
cv::Mat prevTvec = (cv::Mat_<double>(1,3) << (double)prevGuess.x(), (double)prevGuess.y(), (double)prevGuess.z());
cv::projectPoints(objectPoints, prevRvec, prevTvec, K, cv::Mat(), prevImagePoints);
// compute current projection
UDEBUG("project points to previous image");
cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), imagePoints);
//filter points not in the image and set guess from unique correspondences
std::vector<cv::Point3f> objectPointsTmp(objectPoints.size());
std::vector<cv::Point2f> refCorners(objectPoints.size());
std::vector<cv::Point2f> newCorners(objectPoints.size());
matches.resize(objectPoints.size());
int oi=0;
for(unsigned int i=0; i<objectPoints.size(); ++i)
{
if(uIsInBounds(int(imagePoints[i].x), 0, newFrame.cols) &&
uIsInBounds(int(imagePoints[i].y), 0, newFrame.rows) &&
uIsInBounds(int(prevImagePoints[i].x), 0, prevS->getImageRaw().cols) &&
uIsInBounds(int(prevImagePoints[i].y), 0, prevS->getImageRaw().rows))
{
refCorners[oi] = prevImagePoints[i];
newCorners[oi] = imagePoints[i];
if(localMap_.count(ids[i]) == 1)
{
if(prevS->getWords().count(ids[i]) == 1)
{
// set guess if unique
refCorners[oi] = prevS->getWords().find(ids[i])->second.pt;
}
if(newS->getWords().count(ids[i]) == 1)
{
// set guess if unique
newCorners[oi] = newS->getWords().find(ids[i])->second.pt;
}
}
objectPointsTmp[oi] = objectPoints[i];
matches[oi] = ids[i];
++oi;
}
}
objectPointsTmp.resize(oi);
refCorners.resize(oi);
newCorners.resize(oi);
matches.resize(oi);
// Refine imagePoints using optical flow
std::vector<unsigned char> statusFlowInliers;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
prevS->getImageRaw(),
newFrame,
refCorners,
newCorners,
statusFlowInliers,
err,
cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS | cv::OPTFLOW_USE_INITIAL_FLOW, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
objectPoints.resize(statusFlowInliers.size());
imagePoints.resize(statusFlowInliers.size());
std::vector<int> matchesTmp(statusFlowInliers.size());
oi = 0;
for(unsigned int i=0; i<statusFlowInliers.size(); ++i)
{
if(statusFlowInliers[i])
{
objectPoints[oi] = objectPointsTmp[i];
imagePoints[oi] = newCorners[i];
matchesTmp[oi] = matches[i];
++oi;
if(this->isInfoDataFilled() && info)
{
cv::KeyPoint kpt;
if(newS->getWords().count(matches[i]) == 1)
{
kpt = newS->getWords().find(matches[i])->second;
}
kpt.pt = newCorners[i];
info->words.insert(std::make_pair(matches[i], kpt));
}
}
}
UDEBUG("Flow inliers= %d/%d", oi, (int)statusFlowInliers.size());
objectPoints.resize(oi);
imagePoints.resize(oi);
matchesTmp.resize(oi);
matches = matchesTmp;
if(this->isInfoDataFilled() && info)
{
info->wordMatches.insert(info->wordMatches.end(), matches.begin(), matches.end());
}
correspondences = (int)matches.size();
if((int)matches.size() < this->getMinInliers())
{
UWARN("not enough matches (%d < %d)...", (int)matches.size(), this->getMinInliers());
}
else
{
//PnPRansac
std::vector<int> inliersV;
cv::solvePnPRansac(
objectPoints,
imagePoints,
K,
cv::Mat(),
rvec,
tvec,
true,
this->getIterations(),
this->getPnPReprojError(),
0,
inliersV,
this->getPnPFlags());
UDEBUG("inliers=%d/%d", (int)inliersV.size(), (int)objectPoints.size());
inliers = (int)inliersV.size();
if((int)inliersV.size() < this->getMinInliers())
{
UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
}
else
{
cv::Mat R(3,3,CV_64FC1);
cv::Rodrigues(rvec, R);
Transform pnp = Transform(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));
output = this->getPose().inverse() * pnp.inverse() * data.localTransform().inverse();
if(this->isInfoDataFilled() && info && inliersV.size())
{
info->wordInliers.resize(inliersV.size());
for(unsigned int i=0; i<inliersV.size(); ++i)
{
info->wordInliers[i] = matches[inliersV[i]]; // index and ID should match (index starts at 0, ID starts at 1)
}
}
//Find the frame with the most similar features
std::set<int> stMem = memory_->getStMem();
stMem.erase(newS->id());
std::map<int, float> likelihood = memory_->computeLikelihood(newS, std::list<int>(stMem.begin(), stMem.end()));
int maxLikelihoodId = -1;
float maxLikelihood = 0;
for(std::map<int, float>::iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter)
{
if(iter->second > maxLikelihood)
{
maxLikelihood = iter->second;
maxLikelihoodId = iter->first;
}
}
UASSERT(maxLikelihoodId != -1);
// Add new points to local map
const Signature* previousS = memory_->getSignature(maxLikelihoodId);
UASSERT(previousS!=0);
Transform cameraTransform = keyFramePoses_.at(previousS->id()).inverse()*this->getPose()*output;
UDEBUG("cameraTransform guess= %s (norm^2=%f)", cameraTransform.prettyPrint().c_str(), cameraTransform.getNormSquared());
if(cameraTransform.getNorm() < minTranslation_)
{
UWARN("Translation with the nearest frame is too small (%f<%f) to add new points to local map",
cameraTransform.getNorm(), minTranslation_);
}
else
{
double variance = 0;
const std::multimap<int, pcl::PointXYZ> & previousGuess = keyFrameWords3D_.find(previousS->id())->second;
std::multimap<int, pcl::PointXYZ> inliers3D = util3d::generateWords3DMono(
previousS->getWords(),
newS->getWords(),
data.fx(), data.fy()?data.fy():data.fx(),
data.cx(), data.cy(),
data.localTransform(),
cameraTransform,
this->getIterations(),
this->getPnPReprojError(),
this->getPnPFlags(),
fundMatrixReprojError_,
fundMatrixConfidence_,
previousGuess,
&variance);
if((int)inliers3D.size() < this->getMinInliers())
{
UWARN("Epipolar geometry not enough inliers (%d < %d), rejecting the transform (%s)...",
(int)inliers3D.size(), this->getMinInliers(), cameraTransform.prettyPrint().c_str());
}
else if(variance == 0 || variance > maxVariance_)
{
UWARN("Variance too high %f (max = %f)", variance, maxVariance_);
}
else
{
UDEBUG("inliers3D=%d/%d variance= %f", inliers3D.size(), newS->getWords().size(), variance);
Transform newPose = keyFramePoses_.at(previousS->id())*cameraTransform;
UDEBUG("cameraTransform= %s", cameraTransform.prettyPrint().c_str());
std::multimap<int, cv::Point3f> wordsToAdd;
for(std::multimap<int, pcl::PointXYZ>::iterator iter=inliers3D.begin();
iter != inliers3D.end();
++iter)
{
// transform inliers3D in new signature referential
iter->second = util3d::transformPoint(iter->second, cameraTransform.inverse());
if(!uContains(localMap_, iter->first))
{
//UDEBUG("Add new point %d to local map", iter->first);
pcl::PointXYZ newPt = util3d::transformPoint(iter->second, newPose);
wordsToAdd.insert(std::make_pair(iter->first, cv::Point3f(newPt.x, newPt.y, newPt.z)));
}
}
if((int)wordsToAdd.size())
{
localMap_.insert(wordsToAdd.begin(), wordsToAdd.end());
newPtsAdded = true;
UDEBUG("Added %d words", (int)wordsToAdd.size());
}
if(newPtsAdded)
{
keyFrameWords3D_.insert(std::make_pair(newS->id(), inliers3D));
keyFramePoses_.insert(std::make_pair(newS->id(), newPose));
// keep only the two last signatures
while(localHistoryMaxSize_ && (int)localMap_.size() > localHistoryMaxSize_ && memory_->getStMem().size()>2)
{
int nodeId = *memory_->getStMem().begin();
std::list<int> removedPts;
memory_->deleteLocation(nodeId, &removedPts);
keyFrameWords3D_.erase(nodeId);
keyFramePoses_.erase(nodeId);
for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
{
localMap_.erase(*iter);
}
}
}
}
}
}
}
}
if(!newPtsAdded)
{
// remove new words from dictionary
memory_->deleteLocation(newS->id());
}
}
}
else if(cornersMap_.size())
{
//flow
if(this->isInfoDataFilled() && info)
{
info->type = 1;
}
const Signature * refS = memory_->getLastWorkingSignature();
std::vector<cv::Point2f> refCorners(cornersMap_.size());
std::vector<cv::Point2f> refCornersGuess(cornersMap_.size());
std::vector<int> cornerIds(cornersMap_.size());
int ii=0;
for(std::map<int, cv::Point2f>::iterator iter=cornersMap_.begin(); iter!=cornersMap_.end(); ++iter)
{
std::multimap<int, cv::KeyPoint>::const_iterator jter=refS->getWords().find(iter->first);
UASSERT(jter != refS->getWords().end());
refCorners[ii] = jter->second.pt;
refCornersGuess[ii] = iter->second;
cornerIds[ii] = iter->first;
++ii;
}
UDEBUG("flow");
// Find features in the new left image
std::vector<unsigned char> statusFlowInliers;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
refS->getImageRaw(),
newFrame,
refCorners,
refCornersGuess,
statusFlowInliers,
err,
cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS | cv::OPTFLOW_USE_INITIAL_FLOW, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
UDEBUG("Filtering optical flow outliers...");
float flow = 0;
if(this->isInfoDataFilled() && info)
{
info->refCorners = refCorners;
info->newCorners = refCornersGuess;
}
int oi = 0;
std::vector<cv::Point2f> tmpRefCorners(statusFlowInliers.size());
std::vector<cv::Point2f> newCorners(statusFlowInliers.size());
std::vector<int> inliersV(statusFlowInliers.size());
std::vector<int> tmpCornersId(statusFlowInliers.size());
UASSERT(refCornersGuess.size() == statusFlowInliers.size());
UASSERT(refCorners.size() == statusFlowInliers.size());
UASSERT(cornerIds.size() == statusFlowInliers.size());
for(unsigned int i=0; i<statusFlowInliers.size(); ++i)
{
if(statusFlowInliers[i])
{
float dx = refCorners[i].x - refCornersGuess[i].x;
float dy = refCorners[i].y - refCornersGuess[i].y;
float tmp = std::sqrt(dx*dx + dy*dy);
flow+=tmp;
tmpRefCorners[oi] = refCorners[i];
newCorners[oi] = refCornersGuess[i];
inliersV[oi] = i;
cornersMap_.at(cornerIds[i]) = refCornersGuess[i];
tmpCornersId[oi] = cornerIds[i];
++oi;
}
else
{
cornersMap_.erase(cornerIds[i]);
}
}
if(oi)
{
flow /=float(oi);
}
tmpRefCorners.resize(oi);
newCorners.resize(oi);
inliersV.resize((oi));
tmpCornersId.resize(oi);
refCorners= tmpRefCorners;
cornerIds = tmpCornersId;
if(this->isInfoDataFilled() && info)
{
// fill flow matches info
info->cornerInliers = inliersV;
inliers = (int)inliersV.size();
}
UDEBUG("Filtering optical flow outliers...done! (inliers=%d/%d)", oi, (int)statusFlowInliers.size());
if(flow > initMinFlow_ && oi > this->getMinInliers())
{
UDEBUG("flow=%f", flow);
// compute fundamental matrix
UDEBUG("Find fundamental matrix");
std::vector<unsigned char> statusFInliers;
cv::Mat F = cv::findFundamentalMat(
refCorners,
newCorners,
statusFInliers,
cv::RANSAC,
fundMatrixReprojError_,
fundMatrixConfidence_);
std::cout << "F=" << F << std::endl;
if(!F.empty())
{
UDEBUG("Filtering fundamental matrix outliers...");
std::vector<cv::Point2f> tmpNewCorners(statusFInliers.size());
std::vector<cv::Point2f> tmpRefCorners(statusFInliers.size());
tmpCornersId.resize(statusFInliers.size());
oi = 0;
UASSERT(newCorners.size() == statusFInliers.size());
UASSERT(refCorners.size() == statusFInliers.size());
UASSERT(cornerIds.size() == statusFInliers.size());
std::vector<int> tmpInliers(statusFInliers.size());
for(unsigned int i=0; i<statusFInliers.size(); ++i)
{
if(statusFInliers[i])
{
tmpNewCorners[oi] = newCorners[i];
tmpRefCorners[oi] = refCorners[i];
tmpInliers[oi] = inliersV[i];
tmpCornersId[oi] = cornerIds[i];
++oi;
}
}
tmpInliers.resize(oi);
tmpNewCorners.resize(oi);
tmpRefCorners.resize(oi);
tmpCornersId.resize(oi);
newCorners = tmpNewCorners;
refCorners = tmpRefCorners;
inliersV = tmpInliers;
cornerIds = tmpCornersId;
if(this->isInfoDataFilled() && info)
{
// update inliers
info->cornerInliers = inliersV;
inliers = (int)inliersV.size();
}
UDEBUG("Filtering fundamental matrix outliers...done! (inliers=%d/%d)", oi, (int)statusFInliers.size());
if((int)refCorners.size() > this->getMinInliers())
{
std::vector<cv::Point2f> refCornersRefined;
std::vector<cv::Point2f> newCornersRefined;
//UDEBUG("Correcting matches...");
cv::correctMatches(F, refCorners, newCorners, refCornersRefined, newCornersRefined);
UASSERT(refCorners.size() == refCornersRefined.size());
UASSERT(newCorners.size() == newCornersRefined.size());
refCorners = refCornersRefined;
newCorners = newCornersRefined;
//UDEBUG("Correcting matches...done!");
UDEBUG("Computing P...");
cv::Mat K = (cv::Mat_<double>(3,3) <<
data.fx(), 0, data.cx(),
0, data.fy()==0?data.fx():data.fy(), data.cy(),
0, 0, 1);
cv::Mat Kinv = K.inv();
cv::Mat E = K.t()*F*K;
//normalize coordinates
cv::Mat x(3, refCorners.size(), CV_64FC1);
cv::Mat xp(3, refCorners.size(), CV_64FC1);
for(unsigned int i=0; i<refCorners.size(); ++i)
{
x.at<double>(0, i) = refCorners[i].x;
x.at<double>(1, i) = refCorners[i].y;
x.at<double>(2, i) = 1;
xp.at<double>(0, i) = newCorners[i].x;
xp.at<double>(1, i) = newCorners[i].y;
xp.at<double>(2, i) = 1;
}
cv::Mat x_norm = Kinv * x;
cv::Mat xp_norm = Kinv * xp;
x_norm = x_norm.rowRange(0,2);
xp_norm = xp_norm.rowRange(0,2);
cv::Mat P = EpipolarGeometry::findPFromE(E, x_norm, xp_norm);
if(!P.empty())
{
cv::Mat P0 = cv::Mat::zeros(3, 4, CV_64FC1);
P0.at<double>(0,0) = 1;
P0.at<double>(1,1) = 1;
P0.at<double>(2,2) = 1;
UDEBUG("Computing P...done!");
std::cout << "P=" << P << std::endl;
cv::Mat R, T;
EpipolarGeometry::findRTFromP(P, R, T);
UDEBUG("");
std::vector<double> reprojErrors;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
EpipolarGeometry::triangulatePoints(x_norm, xp_norm, P0, P, cloud, reprojErrors);
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRef(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRefGuess(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<cv::Point2f> imagePoints(cloud->size());
inliersRef->resize(cloud->size());
inliersRefGuess->resize(cloud->size());
tmpCornersId.resize(cloud->size());
oi = 0;
UASSERT(newCorners.size() == cloud->size());
for(unsigned int i=0; i<cloud->size(); ++i)
{
if(cloud->at(i).z>0)
{
imagePoints[oi] = newCorners[i];
tmpCornersId[oi] = cornerIds[i];
(*inliersRef)[oi] = cloud->at(i);
if(!refDepth_.empty())
{
(*inliersRefGuess)[oi] = util3d::projectDepthTo3D(refDepth_, refCorners[i].x, refCorners[i].y, data.cx(), data.cy(), data.fx(), data.fy(), true);
}
++oi;
}
}
imagePoints.resize(oi);
inliersRef->resize(oi);
inliersRefGuess->resize(oi);
tmpCornersId.resize(oi);
cornerIds = tmpCornersId;
bool reject = false;
//estimate scale
float scale = 1;
std::multimap<float, float> scales; // <variance, scale>
if(!refDepth_.empty()) // scale known
{
UASSERT(inliersRefGuess->size() == inliersRef->size());
for(unsigned int i=0; i<inliersRef->size(); ++i)
{
if(pcl::isFinite(inliersRefGuess->at(i)))
{
float s = inliersRefGuess->at(i).z/inliersRef->at(i).z;
std::vector<float> errorSqrdDists(inliersRef->size());
for(unsigned int j=0; j<inliersRef->size(); ++j)
{
if(cloud->at(j).z>0)
{
pcl::PointXYZ refPt = inliersRef->at(j);
refPt.x *= s;
refPt.y *= s;
refPt.z *= s;
const pcl::PointXYZ & guess = inliersRefGuess->at(j);
errorSqrdDists[j] = uNormSquared(refPt.x-guess.x, refPt.y-guess.y, refPt.z-guess.z);
}
}
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
float variance = 2.1981 * median_error_sqr;
//UDEBUG("scale %d = %f variance = %f", i, s, variance);
if(variance > 0)
{
scales.insert(std::make_pair(variance, s));
}
}
}
UASSERT(scales.size());
scale = scales.begin()->second;
UDEBUG("scale used = %f (variance=%f)", scale, scales.begin()->first);
maxVariance_ = 0.01;
UDEBUG("Max noise variance = %f current variance=%f", 0.01, scales.begin()->first);
if(scales.begin()->first > 0.01)
{
UWARN("Too high variance %f (should be < 0.01)");
reject = true; // 20 cm for good initialization
}
}
else if(inliersRef->size())
{
// find centroid of the cloud and set it to 1 meter
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*inliersRef, centroid);
scale = 1.0f / centroid[2];
maxVariance_ = 0.01;
}
else
{
reject = true;
}
if(!reject)
{
//PnPRansac
std::vector<cv::Point3f> objectPoints(inliersRef->size());
for(unsigned int i=0; i<inliersRef->size(); ++i)
{
objectPoints[i].x = inliersRef->at(i).x * scale;
objectPoints[i].y = inliersRef->at(i).y * scale;
objectPoints[i].z = inliersRef->at(i).z * scale;
}
cv::Mat rvec;
cv::Mat tvec;
std::vector<int> inliersPnP;
cv::solvePnPRansac(
objectPoints, // 3D points in ref referential
imagePoints, // 2D points in new referential
K,
cv::Mat(),
rvec,
tvec,
false,
this->getIterations(),
this->getPnPReprojError(),
0,
inliersPnP,
this->getPnPFlags());
UDEBUG("PnP inliers = %d / %d", (int)inliersPnP.size(), (int)objectPoints.size());
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));
output = data.localTransform() * pnp.inverse() * data.localTransform().inverse();
if(output.getNorm() < minTranslation_*5)
{
reject = true;
UWARN("Camera must be moved at least %f m for initialization (current=%f)",
minTranslation_*5, output.getNorm());
}
if(!reject)
{
///
std::vector<int> wordsId = uKeys(memory_->getLastWorkingSignature()->getWords());
UASSERT(wordsId.size());
UASSERT(cornerIds.size() == objectPoints.size());
std::multimap<int, pcl::PointXYZ> keyFrameWords3D;
for(unsigned int i=0; i<inliersPnP.size(); ++i)
{
int index =inliersPnP.at(i);
int id = cornerIds[index];
UASSERT(id > 0 && id <= *wordsId.rbegin());
pcl::PointXYZ pt = util3d::transformPoint(pcl::PointXYZ(objectPoints.at(index).x, objectPoints.at(index).y, objectPoints.at(index).z), this->getPose()*data.localTransform());
localMap_.insert(std::make_pair(id, cv::Point3f(pt.x, pt.y, pt.z)));
keyFrameWords3D.insert(std::make_pair(id, pt));
}
keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), keyFrameWords3D));
keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), this->getPose()));
}
}
}
else
{
UERROR("No valid camera matrix found!");
}
}
else
{
UWARN("Not enough inliers %d/%d", (int)refCorners.size(), this->getMinInliers());
}
}
else
{
UWARN("Fundamental matrix not found!");
}
}
else
{
UWARN("Flow not enough high! flow=%f ki=%d", flow, oi);
}
}
}
else
{
//return Identity
output = Transform::getIdentity();
// generate kpts
if(memory_->update(SensorData(newFrame)))
{
const std::multimap<int, cv::KeyPoint> & words = memory_->getLastWorkingSignature()->getWords();
if((int)words.size() > this->getMinInliers())
{
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
cornersMap_.insert(std::make_pair(iter->first, iter->second.pt));
}
refDepth_ = data.depth().clone();
keyFramePoses_.insert(std::make_pair(memory_->getLastSignatureId(), Transform::getIdentity()));
}
else
{
UWARN("Too low 2D corners (%d), ignoring new frame...",
(int)words.size());
memory_->deleteLocation(memory_->getLastSignatureId());
}
}
else
{
UERROR("Failed creating signature");
}
}
memory_->emptyTrash();
if(this->isInfoDataFilled() && info)
{
//info->variance = variance;
info->inliers = inliers;
info->matches = correspondences;
info->features = nFeatures;
info->localMapSize = (int)localMap_.size();
info->localMap = localMap_;
}
UINFO("Odom update=%fs tf=[%s] inliers=%d/%d, local_map[%d]=%d, accepted=%s",
timer.elapsed(),
output.prettyPrint().c_str(),
inliers,
correspondences,
(int)memory_->getStMem().size(),
(int)localMap_.size(),
!output.isNull()?"true":"false");
return output;
}
} // namespace rtabmap

951
corelib/src/OdometryOpticalFlow.cpp Normal file
View File

@@ -0,0 +1,951 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Odometry.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UStl.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>
namespace rtabmap {
OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
Odometry(parameters),
flowWinSize_(Parameters::defaultOdomFlowWinSize()),
flowIterations_(Parameters::defaultOdomFlowIterations()),
flowEps_(Parameters::defaultOdomFlowEps()),
flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
stereoWinSize_(Parameters::defaultStereoWinSize()),
stereoIterations_(Parameters::defaultStereoIterations()),
stereoEps_(Parameters::defaultStereoEps()),
stereoMaxLevel_(Parameters::defaultStereoMaxLevel()),
stereoMaxSlope_(Parameters::defaultStereoMaxSlope()),
subPixWinSize_(Parameters::defaultOdomSubPixWinSize()),
subPixIterations_(Parameters::defaultOdomSubPixIterations()),
subPixEps_(Parameters::defaultOdomSubPixEps()),
refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
{
Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
Parameters::parse(parameters, Parameters::kStereoWinSize(), stereoWinSize_);
Parameters::parse(parameters, Parameters::kStereoIterations(), stereoIterations_);
Parameters::parse(parameters, Parameters::kStereoEps(), stereoEps_);
Parameters::parse(parameters, Parameters::kStereoMaxLevel(), stereoMaxLevel_);
Parameters::parse(parameters, Parameters::kStereoMaxSlope(), stereoMaxSlope_);
Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize_);
Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations_);
Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps_);
ParametersMap::const_iterator iter;
Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
if((iter=parameters.find(Parameters::kOdomFeatureType())) != parameters.end())
{
detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
}
ParametersMap customParameters;
int maxFeatures = Parameters::defaultOdomMaxFeatures();
Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
// add only feature stuff
for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string group = uSplit(iter->first, '/').front();
if(group.compare("SURF") == 0 ||
group.compare("SIFT") == 0 ||
group.compare("BRIEF") == 0 ||
group.compare("FAST") == 0 ||
group.compare("ORB") == 0 ||
group.compare("FREAK") == 0 ||
group.compare("GFTT") == 0 ||
group.compare("BRISK") == 0)
{
customParameters.insert(*iter);
}
}
feature2D_ = Feature2D::create(detectorStrategy, customParameters);
}
OdometryOpticalFlow::~OdometryOpticalFlow()
{
delete feature2D_;
}
void OdometryOpticalFlow::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
refFrame_ = cv::Mat();
refCorners_.clear();
refCorners3D_->clear();
}
// return not null transform if odometry is correctly computed
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;
double variance = 0;
int inliers = 0;
int correspondences = 0;
cv::Mat newLeftFrame;
// convert to grayscale
if(data.image().channels() > 1)
{
cv::cvtColor(data.image(), newLeftFrame, cv::COLOR_BGR2GRAY);
}
else
{
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("");
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
refFrame_,
newLeftFrame,
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");
std::vector<cv::Point2f> lastCornersKept(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];
newCornersKept[ki] = newCorners[i];
++ki;
}
}
lastCornersKept.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());
}
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;
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,
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
{
UDEBUG("Getting correspondences begin");
// Get 3D correspondences
pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(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;
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(statusLast.size());
info->newCorners.resize(statusLast.size());
}
for(unsigned int i=0; i<statusLast.size(); ++i)
{
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());
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())))
{
//Add 3D correspondences!
lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
newPt3D = util3d::transformPoint(newPt3D, data.localTransform());
correspondencesLast->at(oi) = lastPt3D;
correspondencesNew->at(oi) = newPt3D;
if(this->isInfoDataFilled() && info)
{
info->refCorners[oi] = lastCornersKept[i];
info->newCorners[oi] = newCornersKept[i];
}
++oi;
}
}
}
}// end loop
correspondencesLast->resize(oi);
correspondencesNew->resize(oi);
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(oi);
info->newCorners.resize(oi);
}
correspondences = oi;
refCorners3D_ = correspondencesNew;
UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)statusLast.size());
if(correspondences >= this->getMinInliers())
{
std::vector<int> inliersV;
UTimer timerRANSAC;
Transform t = 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(!t.isNull() && inliers >= this->getMinInliers())
{
output = t;
}
else
{
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(newLeftFrame, this->getRoiRatios());
newKtps = feature2D_->generateKeypoints(newLeftFrame, roi);
if(newKtps.size())
{
cv::KeyPoint::convert(newKtps, newCorners);
if(subPixWinSize_ > 0 && subPixIterations_ > 0)
{
UDEBUG("cv::cornerSubPix() begin");
cv::cornerSubPix(newLeftFrame, newCorners,
cv::Size( subPixWinSize_, subPixWinSize_ ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
UDEBUG("cv::cornerSubPix() end");
}
}
}
if((int)newCorners.size() > this->getMinInliers())
{
refFrame_ = newLeftFrame;
refRightFrame_ = newRightFrame;
refCorners_ = newCorners;
}
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, new corners=%d, transform accepted=%s",
timer.elapsed(),
output.isNull()?"true":"false",
inliers,
correspondences,
(int)newCorners.size(),
!output.isNull()?"true":"false");
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

152
corelib/src/OdometryThread.cpp Normal file
View File

@@ -0,0 +1,152 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/OdometryThread.h"
#include "rtabmap/core/Odometry.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/utilite/ULogger.h"
namespace rtabmap {
OdometryThread::OdometryThread(Odometry * odometry) :
_odometry(odometry),
_resetOdometry(false)
{
UASSERT(_odometry != 0);
}
OdometryThread::~OdometryThread()
{
this->unregisterFromEventsManager();
this->join(true);
if(_odometry)
{
delete _odometry;
}
UDEBUG("");
}
void OdometryThread::handleEvent(UEvent * event)
{
if(this->isRunning())
{
if(event->getClassName().compare("CameraEvent") == 0)
{
CameraEvent * cameraEvent = (CameraEvent*)event;
if(cameraEvent->getCode() == CameraEvent::kCodeImageDepth)
{
this->addData(cameraEvent->data());
}
else if(cameraEvent->getCode() == CameraEvent::kCodeNoMoreImages)
{
this->post(new CameraEvent()); // forward the event
}
}
else if(event->getClassName().compare("OdometryResetEvent") == 0)
{
_resetOdometry = true;
}
}
}
void OdometryThread::mainLoopKill()
{
_dataAdded.release();
}
//============================================================
// MAIN LOOP
//============================================================
void OdometryThread::mainLoop()
{
if(_resetOdometry)
{
_odometry->reset();
_resetOdometry = false;
}
SensorData data;
getData(data);
if(data.isValid())
{
OdometryInfo info;
Transform pose = _odometry->process(data, &info);
data.setPose(pose, info.variance, info.variance); // a null pose notify that odometry could not be computed
this->post(new OdometryEvent(data, info));
}
}
void OdometryThread::addData(const SensorData & data)
{
if(dynamic_cast<OdometryMono*>(_odometry) == 0)
{
if(data.image().empty() || data.depthOrRightImage().empty() || data.fx() == 0.0f || data.fyOrBaseline() == 0.0f)
{
ULOGGER_ERROR("Missing some information (images empty or missing calibration)!?");
return;
}
}
else
{
if(data.image().empty() || data.fx() == 0.0f || data.fyOrBaseline() == 0.0f)
{
ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
return;
}
}
bool notify = true;
_dataMutex.lock();
{
notify = !_dataBuffer.isValid();
_dataBuffer = data;
}
_dataMutex.unlock();
if(notify)
{
_dataAdded.release();
}
}
void OdometryThread::getData(SensorData & data)
{
_dataAdded.acquire();
_dataMutex.lock();
{
if(_dataBuffer.isValid())
{
data = _dataBuffer;
_dataBuffer = SensorData();
}
}
_dataMutex.unlock();
}
} // namespace rtabmap

4
corelib/src/SensorData.cpp
View File

@@ -99,10 +99,10 @@ SensorData::SensorData(const cv::Mat & image,
{
UASSERT(image.type() == CV_8UC1 || // Mono
image.type() == CV_8UC3); // RGB
UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter
UASSERT(depthOrRightImage.empty() ||
depthOrRightImage.type() == CV_32FC1 || // Depth in meter
depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
depthOrRightImage.type() == CV_8U); // Right stereo image
UASSERT(!depthOrRightImage.empty());
UASSERT(!_localTransform.isNull());
UASSERT_MSG(uIsFinite(_poseRotVariance) && _poseRotVariance>0 && uIsFinite(_poseTransVariance) && _poseTransVariance>0, "Rotational and transitional variances should not be null! (set to 1 if unknown)");
}

5
corelib/src/Signature.cpp
View File

@@ -246,6 +246,11 @@ void Signature::setDepthCompressed(const cv::Mat & bytes, float fx, float fy, fl
_cy=cy;
}
float Signature::getDepthFx() const {return getFx();}
float Signature::getDepthFy() const {return getFy();}
float Signature::getDepthCx() const {return getCx();}
float Signature::getDepthCy() const {return getCy();}
SensorData Signature::toSensorData()
{
this->uncompressData();

300
corelib/src/util3d.cpp
View File

@@ -404,6 +404,286 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
return keypoints3d;
}
// cameraTransform, from ref to next
// return 3D points in ref referential
// If cameraTransform is not null, it will be used for triangulation instead of the camera transform computed by epipolar geometry
// when refGuess3D is passed and cameraTransform is null, scale will be estimated, returning scaled cloud and camera transform
std::multimap<int, pcl::PointXYZ> generateWords3DMono(
const std::multimap<int, cv::KeyPoint> & refWords,
const std::multimap<int, cv::KeyPoint> & nextWords,
float fx,
float fy,
float cx,
float cy,
const Transform & localTransform,
Transform & cameraTransform,
int pnpIterations,
float pnpReprojError,
int pnpFlags,
float ransacParam1,
float ransacParam2,
const std::multimap<int, pcl::PointXYZ> & refGuess3D,
double * varianceOut)
{
std::multimap<int, pcl::PointXYZ> words3D;
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
if(EpipolarGeometry::findPairsUnique(refWords, nextWords, pairs) > 8)
{
std::vector<unsigned char> status;
cv::Mat F = EpipolarGeometry::findFFromWords(pairs, status, ransacParam1, ransacParam2);
if(!F.empty())
{
//get inliers
//normalize coordinates
int oi = 0;
UASSERT(status.size() == pairs.size());
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin();
std::vector<cv::Point2f> refCorners(status.size());
std::vector<cv::Point2f> newCorners(status.size());
std::vector<int> indexes(status.size());
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i])
{
refCorners[oi] = iter->second.first.pt;
newCorners[oi] = iter->second.second.pt;
indexes[oi] = iter->first;
++oi;
}
++iter;
}
refCorners.resize(oi);
newCorners.resize(oi);
indexes.resize(oi);
UDEBUG("inliers=%d/%d", oi, pairs.size());
if(oi > 3)
{
std::vector<cv::Point2f> refCornersRefined;
std::vector<cv::Point2f> newCornersRefined;
cv::correctMatches(F, refCorners, newCorners, refCornersRefined, newCornersRefined);
refCorners = refCornersRefined;
newCorners = newCornersRefined;
cv::Mat x(3, refCorners.size(), CV_64FC1);
cv::Mat xp(3, refCorners.size(), CV_64FC1);
for(unsigned int i=0; i<refCorners.size(); ++i)
{
x.at<double>(0, i) = refCorners[i].x;
x.at<double>(1, i) = refCorners[i].y;
x.at<double>(2, i) = 1;
xp.at<double>(0, i) = newCorners[i].x;
xp.at<double>(1, i) = newCorners[i].y;
xp.at<double>(2, i) = 1;
}
cv::Mat K = (cv::Mat_<double>(3,3) <<
fx, 0, cx,
0, fy, cy,
0, 0, 1);
cv::Mat Kinv = K.inv();
cv::Mat E = K.t()*F*K;
cv::Mat x_norm = Kinv * x;
cv::Mat xp_norm = Kinv * xp;
x_norm = x_norm.rowRange(0,2);
xp_norm = xp_norm.rowRange(0,2);
cv::Mat P = EpipolarGeometry::findPFromE(E, x_norm, xp_norm);
if(!P.empty())
{
cv::Mat P0 = cv::Mat::zeros(3, 4, CV_64FC1);
P0.at<double>(0,0) = 1;
P0.at<double>(1,1) = 1;
P0.at<double>(2,2) = 1;
bool useCameraTransformGuess = !cameraTransform.isNull();
//if camera transform is set, use it instead of the computed one from epipolar geometry
if(useCameraTransformGuess)
{
Transform t = (localTransform.inverse()*cameraTransform*localTransform).inverse();
P = (cv::Mat_<double>(3,4) <<
(double)t.r11(), (double)t.r12(), (double)t.r13(), (double)t.x(),
(double)t.r21(), (double)t.r22(), (double)t.r23(), (double)t.y(),
(double)t.r31(), (double)t.r32(), (double)t.r33(), (double)t.z());
}
// triangulate the points
//std::vector<double> reprojErrors;
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
//EpipolarGeometry::triangulatePoints(x_norm, xp_norm, P0, P, cloud, reprojErrors);
cv::Mat pts4D;
cv::triangulatePoints(P0, P, x_norm, xp_norm, pts4D);
for(unsigned int i=0; i<indexes.size(); ++i)
{
//if(cloud->at(i).z > 0)
//{
// words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(cloud->at(i), localTransform)));
//}
pts4D.col(i) /= pts4D.at<double>(3,i);
if(pts4D.at<double>(2,i) > 0)
{
words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(pcl::PointXYZ(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), localTransform)));
}
}
if(!useCameraTransformGuess)
{
cv::Mat R, T;
EpipolarGeometry::findRTFromP(P, R, T);
Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2));
cameraTransform = (localTransform * t).inverse() * localTransform;
}
if(refGuess3D.size())
{
// scale estimation
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRef(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersRefGuess(new pcl::PointCloud<pcl::PointXYZ>);
util3d::findCorrespondences(
words3D,
refGuess3D,
*inliersRef,
*inliersRefGuess,
0);
if(inliersRef->size())
{
// estimate the scale
float scale = 1.0f;
float variance = 1.0f;
if(!useCameraTransformGuess)
{
std::multimap<float, float> scales; // <variance, scale>
for(unsigned int i=0; i<inliersRef->size(); ++i)
{
// using x as depth, assuming we are in global referential
float s = inliersRefGuess->at(i).x/inliersRef->at(i).x;
std::vector<float> errorSqrdDists(inliersRef->size());
for(unsigned int j=0; j<inliersRef->size(); ++j)
{
pcl::PointXYZ refPt = inliersRef->at(j);
refPt.x *= s;
refPt.y *= s;
refPt.z *= s;
const pcl::PointXYZ & newPt = inliersRefGuess->at(j);
errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
}
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
float var = 2.1981 * median_error_sqr;
//UDEBUG("scale %d = %f variance = %f", (int)i, s, variance);
scales.insert(std::make_pair(var, s));
}
scale = scales.begin()->second;
variance = scales.begin()->first;;
}
else
{
//compute variance at scale=1
std::vector<float> errorSqrdDists(inliersRef->size());
for(unsigned int j=0; j<inliersRef->size(); ++j)
{
const pcl::PointXYZ & refPt = inliersRef->at(j);
const pcl::PointXYZ & newPt = inliersRefGuess->at(j);
errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
}
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
variance = 2.1981 * median_error_sqr;
}
UDEBUG("scale used = %f (variance=%f)", scale, variance);
if(varianceOut)
{
*varianceOut = variance;
}
if(!useCameraTransformGuess)
{
std::vector<cv::Point3f> objectPoints(indexes.size());
std::vector<cv::Point2f> imagePoints(indexes.size());
int oi=0;
for(unsigned int i=0; i<indexes.size(); ++i)
{
std::multimap<int, pcl::PointXYZ>::iterator iter = words3D.find(indexes[i]);
if(pcl::isFinite(iter->second))
{
iter->second.x *= scale;
iter->second.y *= scale;
iter->second.z *= scale;
objectPoints[oi].x = iter->second.x;
objectPoints[oi].y = iter->second.y;
objectPoints[oi].z = iter->second.z;
imagePoints[oi] = newCorners[i];
++oi;
}
}
objectPoints.resize(oi);
imagePoints.resize(oi);
//PnPRansac
Transform guess = 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,
pnpIterations,
pnpReprojError,
0,
inliersV,
pnpFlags);
UDEBUG("PnP inliers = %d / %d", (int)inliersV.size(), (int)objectPoints.size());
if(inliersV.size())
{
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));
cameraTransform = (localTransform * pnp).inverse();
}
else
{
UWARN("No inliers after PnP!");
cameraTransform = Transform();
}
}
}
else
{
UWARN("Cannot compute the scale, no points corresponding between the generated ref words and words guess");
}
}
}
}
}
}
UDEBUG("wordsSet=%d / %d", (int)words3D.size(), (int)refWords.size());
return words3D;
}
std::multimap<int, cv::KeyPoint> aggregate(
const std::list<int> & wordIds,
const std::vector<cv::KeyPoint> & keypoints)
@@ -476,7 +756,7 @@ void findCorrespondences(
pcl::isFinite(inliers2[oi]) &&
(inliers1[oi].x != 0 || inliers1[oi].y != 0 || inliers1[oi].z != 0) &&
(inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
(maxDepth <= 0 || (inliers1[oi].x <= maxDepth && inliers2[oi].x<=maxDepth)))
(maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
{
++oi;
if(uniqueCorrespondences)
@@ -2864,6 +3144,24 @@ cv::Mat decimate(const cv::Mat & image, int decimation)
return out;
}
void savePCDWords(
const std::string & fileName,
const std::multimap<int, pcl::PointXYZ> & words,
const Transform & transform)
{
if(words.size())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.resize(words.size());
int i=0;
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
cloud[i++] = util3d::transformPoint(iter->second, transform);
}
pcl::io::savePCDFile(fileName, cloud);
}
}
}
}