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

Refactoring: removed some code duplication about transformation estimation and features (2D-3D) extraction.

Browse Source 
Added Feature2D::generateKeypoints3D() for convenience.
Added parameter "Vis/PnPOpenCV2".
Added parameter "Vis/ForwardEstOnly".
Removed OdometryOpticalFlow class, replaced by OdometryF2F (frame-to-frame). To get the same previous OpticalFLow approach, parameter "Vis/CorType" should be set to 1.
Some parameters under group "OdomFlow/..." are now under "Vis/CorFlow...".
In Registration class, add computeTransformationMod() method to modify input signatures.
Added constructor Signature(SensorData) for convenience.
Modified words multimap used with cv::Point3f instead of pcl::PointXYZ to limit the use of PCL headers where they are not really required.
Added Stereo::create() for convenience.
Transform: fixed quaternion constructor where data_ was not initialized. Added parentheses operator for convenience.
DatabaseViewer: loading .rtabmap/rtabmap.ini instead of .rtabmap/dbViewer.ini when used from rtabmap application. Added vertical layout option for convenience.
MainWindow: fixed wrong Odometry speed values
ParametersToolBox: using QStackedWidget instead of a QToolBox for space, added "Restore Defaults" button.
This commit is contained in:
matlabbe
2015-12-21 17:27:05 -05:00
parent 3292fb1146
commit 2e9634cf65
54 changed files with 2838 additions and 2725 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/CMakeLists.txt
View File

@@ -56,9 +56,9 @@ SET(SRC_FILES
Odometry.cpp
OdometryThread.cpp
OdometryBOW.cpp
OdometryOpticalFlow.cpp
OdometryMono.cpp
OdometryICP.cpp
OdometryF2F.cpp
Stereo.cpp
StereoDense.cpp

10
corelib/src/DBDriverSqlite3.cpp
View File

@@ -1529,8 +1529,8 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
int visualWordId = 0;
cv::KeyPoint kpt;
std::multimap<int, cv::KeyPoint> visualWords;
std::multimap<int, pcl::PointXYZ> visualWords3;
pcl::PointXYZ depth(0,0,0);
std::multimap<int, cv::Point3f> visualWords3;
cv::Point3f depth(0,0,0);
// Process the result if one
rc = sqlite3_step(ppStmt);
@@ -2305,7 +2305,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
if((*i)->getWords3().size())
{
std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin();
std::multimap<int, pcl::PointXYZ>::const_iterator p=(*i)->getWords3().begin();
std::multimap<int, cv::Point3f>::const_iterator p=(*i)->getWords3().begin();
for(; w!=(*i)->getWords().end(); ++w, ++p)
{
UASSERT(w->first == p->first); // must be same id!
@@ -2316,7 +2316,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures) const
{
for(std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
{
stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, pcl::PointXYZ(0,0,0));
stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, cv::Point3f(0,0,0));
}
}
}
@@ -3008,7 +3008,7 @@ std::string DBDriverSqlite3::queryStepKeypoint() const
{
return "INSERT INTO Map_Node_Word(node_id, word_id, pos_x, pos_y, size, dir, response, depth_x, depth_y, depth_z) VALUES(?,?,?,?,?,?,?,?,?,?);";
}
void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt, int nodeId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const
void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt, int nodeId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt) const
{
if(!ppStmt)
{

3
corelib/src/DBDriverSqlite3.h
View File

@@ -32,7 +32,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/DBDriver.h"
#include <opencv2/features2d/features2d.hpp>
#include "sqlite3/sqlite3.h"
#include <pcl/point_types.h>
namespace rtabmap {
@@ -110,7 +109,7 @@ private:
void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const;
void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt) const;
private:
void loadLinksQuery(std::list<Signature *> & signatures) const;

23
corelib/src/EpipolarGeometry.cpp
View File

@@ -406,6 +406,29 @@ cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy,
return K.inv().t()*E*K.inv();
}
/**
* if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
* realPairsCount = 4
*/
int EpipolarGeometry::findPairs(
const std::map<int, cv::KeyPoint> & wordsA,
const std::map<int, cv::KeyPoint> & wordsB,
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
{
int realPairsCount = 0;
pairs.clear();
for(std::map<int, cv::KeyPoint>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
{
std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
if(ptB != wordsB.end())
{
pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(i->first, std::pair<cv::KeyPoint, cv::KeyPoint>(i->second, ptB->second)));
++realPairsCount;
}
}
return realPairsCount;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
* realPairsCount = 5

255
corelib/src/Features2d.cpp
View File

@@ -27,6 +27,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_features.h"
#include "rtabmap/core/Stereo.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/ULogger.h"
@@ -243,7 +245,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
}
}
}
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)kptsTmp.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
keypoints = kptsTmp;
if(descriptors.rows)
@@ -335,13 +337,74 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> &
// Feature2D
/////////////////////
Feature2D::Feature2D(const ParametersMap & parameters) :
maxFeatures_(Parameters::defaultKpWordsPerImage())
maxFeatures_(Parameters::defaultKpMaxFeatures()),
_wordsMaxDepth(Parameters::defaultKpMaxDepth()),
_wordsMinDepth(Parameters::defaultKpMinDepth()),
_roiRatios(std::vector<float>(4, 0.0f)),
_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
_subPixIterations(Parameters::defaultKpSubPixIterations()),
_subPixEps(Parameters::defaultKpSubPixEps())
{
_stereo = new Stereo(parameters);
this->parseParameters(parameters);
}
Feature2D::~Feature2D()
{
delete _stereo;
}
void Feature2D::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), maxFeatures_);
Parameters::parse(parameters, Parameters::kKpMaxFeatures(), maxFeatures_);
Parameters::parse(parameters, Parameters::kKpMaxDepth(), _wordsMaxDepth);
Parameters::parse(parameters, Parameters::kKpMinDepth(), _wordsMinDepth);
Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
// convert ROI from string to vector
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
{
std::list<std::string> strValues = uSplit(iter->second, ' ');
if(strValues.size() != 4)
{
ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", iter->second.c_str());
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
{
tmpValues[i] = uStr2Float(*iter);
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
_roiRatios = tmpValues;
}
else
{
ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", iter->second.c_str());
}
}
}
//stereo
UASSERT(_stereo != 0);
if((iter=parameters.find(Parameters::kStereoOpticalFlow())) != parameters.end())
{
delete _stereo;
_stereo = Stereo::create(parameters);
}
else
{
_stereo->parseParameters(parameters);
}
}
Feature2D * Feature2D::create(const ParametersMap & parameters)
{
@@ -350,12 +413,6 @@ Feature2D * Feature2D::create(const ParametersMap & parameters)
return create((Feature2D::Type)type, parameters);
}
Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
{
int wordsPerImage = Parameters::defaultKpWordsPerImage();
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), wordsPerImage);
return create(type, wordsPerImage, parameters);
}
Feature2D * Feature2D::create(Feature2D::Type type, int wordsPerImage, const ParametersMap & parameters)
{
if(RTABMAP_NONFREE == 0)
{
@@ -426,54 +483,156 @@ Feature2D * Feature2D::create(Feature2D::Type type, int wordsPerImage, const Par
#endif
}
return feature2D;
}
std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image) const
{
UASSERT(!image.empty());
UASSERT(image.type() == CV_8UC1);
std::vector<cv::KeyPoint> keypoints;
if(!image.empty() && image.channels() == 1 && image.type() == CV_8U)
UTimer timer;
// Get keypoints
cv::Rect roi = Feature2D::computeRoi(image, _roiRatios);
keypoints = this->generateKeypointsImpl(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows));
UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
limitKeypoints(keypoints, maxFeatures_);
if(roi.x || roi.y)
{
UTimer timer;
// Get keypoints
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, maxFeatures_);
if(roi.x || roi.y)
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
{
// Adjust keypoint position to raw image
for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
{
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
iter->pt.x += roi.x;
iter->pt.y += roi.y;
}
}
else if(image.empty())
if(_subPixWinSize > 0 && _subPixIterations > 0)
{
UERROR("Image is null!");
}
else
{
UERROR("Image format must be mono8. Current has %d channels and type = %d, size=%d,%d",
image.channels(), image.type(), image.cols, image.rows);
std::vector<cv::Point2f> corners;
cv::KeyPoint::convert(keypoints, corners);
cv::cornerSubPix( image, 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];
}
UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
}
return keypoints;
}
cv::Mat Feature2D::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
cv::Mat Feature2D::generateDescriptors(
const cv::Mat & image,
std::vector<cv::KeyPoint> & keypoints) const
{
UASSERT(!image.empty());
UASSERT(image.type() == CV_8UC1);
cv::Mat descriptors = generateDescriptorsImpl(image, keypoints);
UASSERT_MSG(descriptors.rows == (int)keypoints.size(), uFormat("descriptors=%d, keypoints=%d", descriptors.rows, (int)keypoints.size()).c_str());
UDEBUG("Descriptors extracted = %d, remaining kpts=%d", descriptors.rows, (int)keypoints.size());
return descriptors;
}
std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
const SensorData & data,
const std::vector<cv::KeyPoint> & keypoints) const
{
std::vector<cv::Point3f> keypoints3D;
if(!data.depthOrRightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValid())
{
//stereo
cv::Mat imageMono;
// convert to grayscale
if(data.imageRaw().channels() > 1)
{
cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY);
}
else
{
imageMono = data.imageRaw();
}
//generate a disparity map
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
std::vector<unsigned char> status;
std::vector<cv::Point2f> rightCorners;
rightCorners = _stereo->computeCorrespondences(
imageMono,
data.rightRaw(),
leftCorners,
status);
if(_wordsMaxDepth > 0.0f || _wordsMinDepth > 0.0f)
{
UASSERT(status.size() == leftCorners.size() && status.size() == rightCorners.size());
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] != 0)
{
float d = data.stereoCameraModel().computeDepth(leftCorners[i].x - rightCorners[i].x);
if((_wordsMinDepth > 0.0f && d < _wordsMinDepth) ||
(_wordsMaxDepth > 0.0f && d > _wordsMaxDepth))
{
status[i] = 0;
}
}
}
}
keypoints3D = util3d::generateKeypoints3DStereo(
leftCorners,
rightCorners,
data.stereoCameraModel(),
status);
}
else if(!data.depthRaw().empty() && data.cameraModels().size())
{
keypoints3D = util3d::generateKeypoints3DDepth(
keypoints,
data.depthOrRightRaw(),
data.cameraModels());
if(_wordsMaxDepth > 0.0f || _wordsMinDepth > 0.0f)
{
UASSERT(keypoints3D.size() == keypoints.size());
bool isInMM = data.depthRaw().type() == CV_16UC1;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(unsigned int i=0; i<keypoints.size(); ++i)
{
int u = int(keypoints[i].pt.x+0.5f);
int v = int(keypoints[i].pt.y+0.5f);
bool reject = true;
if(u >=0 && u<data.depthRaw().cols && v >=0 && v<data.depthRaw().rows)
{
float d = isInMM?(float)data.depthRaw().at<uint16_t>(v,u)*0.001f:data.depthRaw().at<float>(v,u);
if(uIsFinite(d) && d>_wordsMinDepth && (_wordsMaxDepth <= 0.0f || d < _wordsMaxDepth))
{
reject = false;
}
}
if(reject)
{
keypoints3D[i].x = bad_point;
keypoints3D[i].y = bad_point;
keypoints3D[i].z = bad_point;
}
}
}
}
return keypoints3D;
}
//////////////////////////
//SURF
//////////////////////////
@@ -605,7 +764,6 @@ cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
//SIFT
//////////////////////////
SIFT::SIFT(const ParametersMap & parameters) :
nfeatures_(Parameters::defaultSIFTNFeatures()),
nOctaveLayers_(Parameters::defaultSIFTNOctaveLayers()),
contrastThreshold_(Parameters::defaultSIFTContrastThreshold()),
edgeThreshold_(Parameters::defaultSIFTEdgeThreshold()),
@@ -624,15 +782,14 @@ void SIFT::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), contrastThreshold_);
Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), edgeThreshold_);
Parameters::parse(parameters, Parameters::kSIFTNFeatures(), nfeatures_);
Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), nOctaveLayers_);
Parameters::parse(parameters, Parameters::kSIFTSigma(), sigma_);
#if RTABMAP_NONFREE == 1
#if CV_MAJOR_VERSION < 3
_sift = cv::Ptr<CV_SIFT>(new CV_SIFT(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
_sift = cv::Ptr<CV_SIFT>(new CV_SIFT(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_));
#else
_sift = CV_SIFT::create(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
_sift = CV_SIFT::create(this->getMaxFeatures(), nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
#endif
#else
UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
@@ -668,7 +825,6 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
//ORB
//////////////////////////
ORB::ORB(const ParametersMap & parameters) :
nFeatures_(Parameters::defaultKpWordsPerImage()),
scaleFactor_(Parameters::defaultORBScaleFactor()),
nLevels_(Parameters::defaultORBNLevels()),
edgeThreshold_(Parameters::defaultORBEdgeThreshold()),
@@ -691,7 +847,6 @@ void ORB::parseParameters(const ParametersMap & parameters)
{
Feature2D::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), nFeatures_);
Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor_);
Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels_);
Parameters::parse(parameters, Parameters::kORBEdgeThreshold(), edgeThreshold_);
@@ -727,7 +882,7 @@ void ORB::parseParameters(const ParametersMap & parameters)
if(gpu_)
{
#if CV_MAJOR_VERSION < 3
_gpuOrb = cv::Ptr<CV_ORB_GPU>(new CV_ORB_GPU(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_));
_gpuOrb = cv::Ptr<CV_ORB_GPU>(new CV_ORB_GPU(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_));
_gpuOrb->setFastParams(fastThreshold_, nonmaxSuppresion_);
#else
#ifdef HAVE_OPENCV_CUDAFEATURES2D
@@ -738,9 +893,9 @@ void ORB::parseParameters(const ParametersMap & parameters)
else
{
#if CV_MAJOR_VERSION < 3
_orb = cv::Ptr<CV_ORB>(new CV_ORB(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_));
_orb = cv::Ptr<CV_ORB>(new CV_ORB(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_));
#else
_orb = CV_ORB::create(nFeatures_, scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_);
_orb = CV_ORB::create(this->getMaxFeatures(), scaleFactor_, nLevels_, edgeThreshold_, firstLevel_, WTA_K_, scoreType_, patchSize_);
#endif
}
}
@@ -821,7 +976,6 @@ FAST::FAST(const ParametersMap & parameters) :
gpuKeypointsRatio_(Parameters::defaultFASTGpuKeypointsRatio()),
minThreshold_(Parameters::defaultFASTMinThreshold()),
maxThreshold_(Parameters::defaultFASTMaxThreshold()),
maxTotalKeypoints_(Parameters::defaultKpWordsPerImage()),
gridRows_(Parameters::defaultFASTGridRows()),
gridCols_(Parameters::defaultFASTGridCols())
{
@@ -843,12 +997,9 @@ void FAST::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kFASTMinThreshold(), minThreshold_);
Parameters::parse(parameters, Parameters::kFASTMaxThreshold(), maxThreshold_);
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), maxTotalKeypoints_);
Parameters::parse(parameters, Parameters::kFASTGridRows(), gridRows_);
Parameters::parse(parameters, Parameters::kFASTGridCols(), gridCols_);
UWARN("minThreshold_=%d", minThreshold_);
UASSERT_MSG(threshold_ >= minThreshold_, uFormat("%d vs %d", threshold_, minThreshold_).c_str());
UASSERT_MSG(threshold_ <= maxThreshold_, uFormat("%d vs %d", threshold_, maxThreshold_).c_str());
@@ -893,7 +1044,7 @@ void FAST::parseParameters(const ParametersMap & parameters)
if(gridRows_ > 0 && gridCols_ > 0)
{
cv::Ptr<cv::FeatureDetector> fastAdjuster = cv::Ptr<cv::FastAdjuster>(new cv::FastAdjuster(threshold_, nonmaxSuppression_, minThreshold_, maxThreshold_));
_fast = cv::Ptr<cv::FeatureDetector>(new cv::GridAdaptedFeatureDetector(fastAdjuster, maxTotalKeypoints_, gridRows_, gridCols_));
_fast = cv::Ptr<cv::FeatureDetector>(new cv::GridAdaptedFeatureDetector(fastAdjuster, this->getMaxFeatures(), gridRows_, gridCols_));
}
else
{
@@ -1067,7 +1218,6 @@ cv::Mat FAST_ORB::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv:
//GFTT
//////////////////////////
GFTT::GFTT(const ParametersMap & parameters) :
_maxCorners(Parameters::defaultKpWordsPerImage()),
_qualityLevel(Parameters::defaultGFTTQualityLevel()),
_minDistance(Parameters::defaultGFTTMinDistance()),
_blockSize(Parameters::defaultGFTTBlockSize()),
@@ -1085,7 +1235,6 @@ void GFTT::parseParameters(const ParametersMap & parameters)
{
Feature2D::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), _maxCorners);
Parameters::parse(parameters, Parameters::kGFTTQualityLevel(), _qualityLevel);
Parameters::parse(parameters, Parameters::kGFTTMinDistance(), _minDistance);
Parameters::parse(parameters, Parameters::kGFTTBlockSize(), _blockSize);
@@ -1093,9 +1242,9 @@ void GFTT::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kGFTTK(), _k);
#if CV_MAJOR_VERSION < 3
_gftt = cv::Ptr<CV_GFTT>(new CV_GFTT(_maxCorners, _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k));
_gftt = cv::Ptr<CV_GFTT>(new CV_GFTT(this->getMaxFeatures(), _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k));
#else
_gftt = CV_GFTT::create(_maxCorners, _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k);
_gftt = CV_GFTT::create(this->getMaxFeatures(), _qualityLevel, _minDistance, _blockSize, _useHarrisDetector ,_k);
#endif
}

435
corelib/src/Memory.cpp
View File

@@ -96,24 +96,14 @@ Memory::Memory(const ParametersMap & parameters) :
_linksChanged(false),
_signaturesAdded(0),
_featureType((Feature2D::Type)Parameters::defaultKpDetectorStrategy()),
_badSignRatio(Parameters::defaultKpBadSignRatio()),
_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
_parallelized(Parameters::defaultKpParallelized()),
_wordsMaxDepth(Parameters::defaultKpMaxDepth()),
_wordsMinDepth(Parameters::defaultKpMinDepth()),
_roiRatios(std::vector<float>(4, 0.0f)),
_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
_subPixIterations(Parameters::defaultKpSubPixIterations()),
_subPixEps(Parameters::defaultKpSubPixEps())
_parallelized(Parameters::defaultKpParallelized())
{
_feature2D = Feature2D::create(_featureType, parameters);
_featureType = _feature2D->getType();
_feature2D = Feature2D::create(parameters);
_vwd = new VWDictionary(parameters);
_registrationVis = new RegistrationVis(parameters);
_registrationIcp = new RegistrationIcp(parameters);
_stereo = new Stereo(parameters);
this->parseParameters(parameters);
}
@@ -383,10 +373,6 @@ Memory::~Memory()
{
delete _registrationIcp;
}
if(_stereo)
{
delete _stereo;
}
}
void Memory::parseParameters(const ParametersMap & parameters)
@@ -435,17 +421,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kKpTfIdfLikelihoodUsed(), _tfIdfLikelihoodUsed);
Parameters::parse(parameters, Parameters::kKpParallelized(), _parallelized);
Parameters::parse(parameters, Parameters::kKpBadSignRatio(), _badSignRatio);
Parameters::parse(parameters, Parameters::kKpMaxDepth(), _wordsMaxDepth);
Parameters::parse(parameters, Parameters::kKpMinDepth(), _wordsMinDepth);
Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
{
this->setRoi((*iter).second);
}
//Keypoint detector
UASSERT(_feature2D != 0);
@@ -461,11 +436,9 @@ void Memory::parseParameters(const ParametersMap & parameters)
{
delete _feature2D;
_feature2D = 0;
_featureType = Feature2D::kFeatureUndef;
}
_feature2D = Feature2D::create(detectorStrategy, parameters);
_featureType = _feature2D->getType();
}
else if(_feature2D)
{
@@ -480,26 +453,6 @@ void Memory::parseParameters(const ParametersMap & parameters)
{
_registrationIcp->parseParameters(parameters);
}
//stereo
UASSERT(_stereo != 0);
if((iter=parameters.find(Parameters::kStereoOpticalFlow())) != parameters.end())
{
bool opticalFlow = uStr2Bool(iter->second);
delete _stereo;
if(opticalFlow)
{
_stereo = new StereoOpticalFlow(parameters);
}
else
{
_stereo = new Stereo(parameters);
}
}
else
{
_stereo->parseParameters(parameters);
}
// do this after all parameters are parsed
// SLAM mode vs Localization mode
@@ -654,39 +607,6 @@ bool Memory::update(
return true;
}
void Memory::setRoi(const std::string & roi)
{
std::list<std::string> strValues = uSplit(roi, ' ');
if(strValues.size() != 4)
{
ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", roi.c_str());
}
else
{
std::vector<float> tmpValues(4);
unsigned int i=0;
for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
{
tmpValues[i] = uStr2Float(*iter);
++i;
}
if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
{
_roiRatios = tmpValues;
}
else
{
ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", roi.c_str());
}
}
}
void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance)
{
UTimer timer;
@@ -2105,6 +2025,7 @@ Transform Memory::computeVisualTransform(
if(fromS && toS)
{
// compute transform fromId -> toId
std::vector<int> inliersV;
if(_reextractLoopClosureFeatures)
{
getNodeData(fromS->id(), true);
@@ -2114,14 +2035,18 @@ Transform Memory::computeVisualTransform(
Signature tmpTo = *toS;
tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
tmpFrom.setWords3(std::multimap<int, pcl::PointXYZ>());
tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
tmpTo.setWords3(std::multimap<int, pcl::PointXYZ>());
return _registrationVis->computeTransformation(tmpFrom, tmpTo, Transform::getIdentity(), rejectedMsg, inliers, variance);
tmpTo.setWords3(std::multimap<int, cv::Point3f>());
transform = _registrationVis->computeTransformation(tmpFrom, tmpTo, Transform::getIdentity(), rejectedMsg, &inliersV, variance);
}
else
{
return _registrationVis->computeTransformation(*fromS, *toS, Transform::getIdentity(), rejectedMsg, inliers, variance);
transform = _registrationVis->computeTransformation(*fromS, *toS, Transform::getIdentity(), rejectedMsg, &inliersV, variance);
}
if(inliers)
{
*inliers = (int)inliersV.size();
}
}
else
@@ -2133,7 +2058,7 @@ Transform Memory::computeVisualTransform(
}
UWARN(msg.c_str());
}
return Transform();
return transform;
}
// compute transform fromId -> toId
@@ -2179,7 +2104,12 @@ Transform Memory::computeIcpTransform(
toS->sensorData().uncompressData(0, 0, &tmp2);
// compute transform fromId -> toId
t = _registrationIcp->computeTransformation(*fromS, *toS, guess, rejectedMsg, inliers, variance, inliersRatio);
std::vector<int> inliersV;
t = _registrationIcp->computeTransformation(fromS->sensorData(), toS->sensorData(), guess, rejectedMsg, &inliersV, variance, inliersRatio);
if(inliers)
{
*inliers = (int)inliersV.size();
}
}
else
{
@@ -2259,8 +2189,12 @@ Transform Memory::computeIcpTransformMulti(
}
Transform guess = poses.at(fromId).inverse() * poses.at(toId);
Signature toS(0, 0, 0, 0, "", toPose, assembledData);
t = _registrationIcp->computeTransformation(*fromS, toS, guess, rejectedMsg, inliers, variance);
std::vector<int> inliersV;
t = _registrationIcp->computeTransformation(fromS->sensorData(), assembledData, guess, rejectedMsg, &inliersV, variance);
if(inliers)
{
*inliers = (int)inliersV.size();
}
}
return t;
@@ -2461,8 +2395,8 @@ void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
{
if(words3D)
{
const std::multimap<int, pcl::PointXYZ> & ref = ss->getWords3();
for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
const std::multimap<int, cv::Point3f> & ref = ss->getWords3();
for(std::multimap<int, cv::Point3f>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
{
//show only valid point according to current parameters
if(pcl::isFinite(jter->second) &&
@@ -2851,7 +2785,7 @@ SensorData Memory::getNodeData(int nodeId, bool uncompressedData, bool keepLoade
void Memory::getNodeWords(int nodeId,
std::multimap<int, cv::KeyPoint> & words,
std::multimap<int, pcl::PointXYZ> & words3)
std::multimap<int, cv::Point3f> & words3)
{
UDEBUG("nodeId=%d", nodeId);
Signature * s = this->_getSignature(nodeId);
@@ -3092,227 +3026,44 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
preUpdateThread.start();
}
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3D(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<cv::Point3f> keypoints3D;
if(data.keypoints().size() == 0)
{
if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
{
// Extract features
cv::Mat imageMono;
// convert to grayscale
if(data.imageRaw().channels() > 1)
if(data.imageRaw().channels() == 3)
{
UDEBUG("convert to grayscale...");
cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY);
cv::cvtColor(data.imageRaw(), imageMono, CV_BGR2GRAY);
}
else
{
imageMono = data.imageRaw();
}
UDEBUG("Set ROI...");
cv::Rect roi = Feature2D::computeRoi(imageMono, _roiRatios);
if(!data.depthOrRightRaw().empty() && data.stereoCameraModel().isValid())
{
//stereo
bool subPixelOn = false;
if(_subPixWinSize > 0 && _subPixIterations > 0)
{
subPixelOn = true;
}
UDEBUG("Generating keypoints...");
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);
keypoints = _feature2D->generateKeypoints(imageMono);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);
if(keypoints.size())
{
// descriptors should be extracted before subpixel
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
if(subPixelOn)
{
cv::cornerSubPix( imageMono, leftCorners,
cv::Size( _subPixWinSize, _subPixWinSize ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _subPixIterations, _subPixEps ) );
for(unsigned int i=0;i<leftCorners.size(); ++i)
{
keypoints[i].pt = leftCorners[i];
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemSubpixel(), t*1000.0f);
UDEBUG("time subpix left kpts=%fs", t);
}
UASSERT(keypoints.size() == leftCorners.size());
//generate a disparity map
std::vector<unsigned char> status;
std::vector<cv::Point2f> rightCorners;
rightCorners = _stereo->computeCorrespondences(
imageMono,
data.rightRaw(),
leftCorners,
status);
if(_wordsMaxDepth > 0.0f || _wordsMinDepth > 0.0f)
{
UASSERT(status.size() == leftCorners.size() && status.size() == rightCorners.size());
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] != 0)
{
float d = data.stereoCameraModel().computeDepth(leftCorners[i].x - rightCorners[i].x);
if((_wordsMinDepth > 0.0f && d < _wordsMinDepth) ||
(_wordsMaxDepth > 0.0f && d > _wordsMaxDepth))
{
status[i] = 0;
}
}
}
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemStereo_correspondences(), t*1000.0f);
UDEBUG("generate disparity = %fs", t);
if(keypoints.size())
{
UASSERT(keypoints.size() == descriptors.rows);
UASSERT(leftCorners.size() == keypoints.size());
keypoints3D = util3d::generateKeypoints3DStereo(
leftCorners,
rightCorners,
data.stereoCameraModel(),
status);
UASSERT(keypoints.size() == keypoints3D->size());
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
}
}
}
else if(!data.depthOrRightRaw().empty() && data.cameraModels().size())
{
//depth
bool subPixelOn = false;
if(_subPixWinSize > 0 && _subPixIterations > 0)
{
subPixelOn = true;
}
UDEBUG("Generating keypoints...");
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);
if(keypoints.size())
{
if(subPixelOn)
{
// descriptors should be extracted before subpixel
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
cv::cornerSubPix( imageMono, leftCorners,
cv::Size( _subPixWinSize, _subPixWinSize ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _subPixIterations, _subPixEps ) );
for(unsigned int i=0;i<leftCorners.size(); ++i)
{
keypoints[i].pt = leftCorners[i];
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemSubpixel(), t*1000.0f);
UDEBUG("time subpix left kpts=%fs", t);
}
if(_wordsMaxDepth > 0.0f || _wordsMinDepth > 0.0f)
{
Feature2D::filterKeypointsByDepth(keypoints, descriptors, data.depthOrRightRaw(), _wordsMinDepth, _wordsMaxDepth);
UDEBUG("filter keypoints by depth (%d)", (int)keypoints.size());
}
if(keypoints.size())
{
if(!subPixelOn)
{
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
}
UASSERT(keypoints.size() == descriptors.rows);
keypoints3D = util3d::generateKeypoints3DDepth(
keypoints,
data.depthOrRightRaw(),
data.cameraModels());
UASSERT(keypoints.size() == keypoints3D->size());
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
UDEBUG("Generating keypoints...");
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);
if(keypoints.size())
{
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
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);
}
}
}
descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
{
descriptors = cv::Mat();
}
else if((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValid()) ||
(!data.rightRaw().empty() && data.stereoCameraModel().isValid()))
{
keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
}
}
else if(data.imageRaw().empty())
{
@@ -3332,79 +3083,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
keypoints = data.keypoints();
descriptors = data.descriptors().clone();
// filter by depth
if(!data.depthOrRightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValid())
{
//stereo
cv::Mat imageMono;
// convert to grayscale
if(data.imageRaw().channels() > 1)
{
cv::cvtColor(data.imageRaw(), imageMono, cv::COLOR_BGR2GRAY);
}
else
{
imageMono = data.imageRaw();
}
//generate a disparity map
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
std::vector<unsigned char> status;
std::vector<cv::Point2f> rightCorners;
rightCorners = _stereo->computeCorrespondences(
imageMono,
data.rightRaw(),
leftCorners,
status);
if(_wordsMaxDepth > 0.0f || _wordsMinDepth > 0.0f)
{
UASSERT(status.size() == leftCorners.size() && status.size() == rightCorners.size());
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] != 0)
{
float d = data.stereoCameraModel().computeDepth(leftCorners[i].x - rightCorners[i].x);
if((_wordsMinDepth > 0.0f && d < _wordsMinDepth) ||
(_wordsMaxDepth > 0.0f && d > _wordsMaxDepth))
{
status[i] = 0;
}
}
}
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemStereo_correspondences(), t*1000.0f);
UDEBUG("generate disparity = %fs", t);
keypoints3D = util3d::generateKeypoints3DStereo(
leftCorners,
rightCorners,
data.stereoCameraModel(),
status);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
}
else if(!data.depthOrRightRaw().empty() && data.cameraModels().size())
{
//depth
if(_wordsMaxDepth > 0.0f || _wordsMinDepth > 0.0f)
{
Feature2D::filterKeypointsByDepth(keypoints, descriptors, _wordsMinDepth, _wordsMaxDepth);
UDEBUG("filter keypoints by depth (%d)", (int)keypoints.size());
}
keypoints3D = util3d::generateKeypoints3DDepth(
keypoints,
data.depthOrRightRaw(),
data.cameraModels());
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), t);
}
keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
}
if(_parallelized)
@@ -3439,11 +3118,11 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
}
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, pcl::PointXYZ> words3D;
std::multimap<int, cv::Point3f> words3D;
if(wordIds.size() > 0)
{
UASSERT(wordIds.size() == keypoints.size());
UASSERT(keypoints3D->size() == 0 || keypoints3D->size() == wordIds.size());
UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
unsigned int i=0;
for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
{
@@ -3459,9 +3138,9 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
{
words.insert(std::pair<int, cv::KeyPoint>(*iter, keypoints[i]));
}
if(keypoints3D->size())
if(keypoints3D.size())
{
words3D.insert(std::pair<int, pcl::PointXYZ>(*iter, keypoints3D->at(i)));
words3D.insert(std::pair<int, cv::Point3f>(*iter, keypoints3D.at(i)));
}
}
}
@@ -3478,9 +3157,9 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
{
Transform cameraTransform = 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(),
std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
uMultimapToMapUnique(words),
uMultimapToMapUnique(previousS->getWords()),
data.cameraModels()[0],
cameraTransform);
@@ -3488,21 +3167,21 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
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);
std::map<int, cv::Point3f>::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)));
words3D.insert(std::make_pair(iter->first, cv::Point3f(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);
UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
}
}

4
corelib/src/Odometry.cpp
View File

@@ -40,6 +40,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_inlierDistance(Parameters::defaultVisInlierDistance()),
_iterations(Parameters::defaultVisIterations()),
_refineIterations(Parameters::defaultVisRefineIterations()),
_minDepth(Parameters::defaultVisMinDepth()),
_maxDepth(Parameters::defaultVisMaxDepth()),
_resetCountdown(Parameters::defaultOdomResetCountdown()),
_force2D(Parameters::defaultVisForce2D()),
@@ -54,6 +55,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_estimationType(Parameters::defaultVisEstimationType()),
_pnpReprojError(Parameters::defaultVisPnPReprojError()),
_pnpFlags(Parameters::defaultVisPnPFlags()),
_pnpOpenCV2(Parameters::defaultVisPnPOpenCV2()),
_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount()),
_kalmanProcessNoise(Parameters::defaultOdomKalmanProcessNoise()),
_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
@@ -68,6 +70,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
Parameters::parse(parameters, Parameters::kVisMinDepth(), _minDepth);
Parameters::parse(parameters, Parameters::kVisMaxDepth(), _maxDepth);
Parameters::parse(parameters, Parameters::kVisRoiRatios(), _roiRatios);
Parameters::parse(parameters, Parameters::kVisForce2D(), _force2D);
@@ -76,6 +79,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _pnpReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _pnpFlags);
Parameters::parse(parameters, Parameters::kVisPnPOpenCV2(), _pnpOpenCV2);
UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);

29
corelib/src/OdometryBOW.cpp
View File

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/Optimizer.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
@@ -59,6 +60,7 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomBowFixedLocalMapPath(), _fixedLocalMapPath);
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(this->getMinDepth())));
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), this->getRoiRatios()));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
@@ -66,18 +68,18 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemSaveDepth16Format(), "false"));
int nn = Parameters::defaultVisNNType();
float nndr = Parameters::defaultVisNNDR();
int nn = Parameters::defaultVisCorNNType();
float nndr = Parameters::defaultVisCorNNDR();
int featureType = Parameters::defaultVisFeatureType();
int maxFeatures = Parameters::defaultVisMaxFeatures();
Parameters::parse(parameters, Parameters::kVisNNType(), nn);
Parameters::parse(parameters, Parameters::kVisNNDR(), nndr);
Parameters::parse(parameters, Parameters::kVisCorNNType(), nn);
Parameters::parse(parameters, Parameters::kVisCorNNDR(), nndr);
Parameters::parse(parameters, Parameters::kVisFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), 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)));
customParameters.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(maxFeatures)));
// Memory's stereo parameters, copy from Odometry
int subPixWinSize = Parameters::defaultVisSubPixWinSize();
@@ -150,8 +152,8 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
if(s)
{
// Transform 3D points accordingly to pose and add them to local map
const std::multimap<int, pcl::PointXYZ> & words3D = s->getWords3();
for(std::multimap<int, pcl::PointXYZ>::const_iterator pointsIter=words3D.begin();
const std::multimap<int, cv::Point3f> & words3D = s->getWords3();
for(std::multimap<int, cv::Point3f>::const_iterator pointsIter=words3D.begin();
pointsIter!=words3D.end();
++pointsIter)
{
@@ -255,6 +257,7 @@ Transform OdometryBOW::computeTransform(
this->getIterations(),
this->getPnPReprojError(),
this->getPnPFlags(),
this->getPnPOpenCV2(),
this->getPose(),
uMultimapToMap(newSignature->getWords3()),
isVarianceFromInliersCount()?0:&variance, // don't compute variance if we use inliers
@@ -356,10 +359,10 @@ Transform OdometryBOW::computeTransform(
// keep old word
if(localMap_.find(*iter) == localMap_.end())
{
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt))
const cv::Point3f & pt = newSignature->getWords3().find(*iter)->second;
if(util3d::isFinite(pt))
{
pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
cv::Point3f pt2 = util3d::transformPoint(pt, t);
localMap_.insert(std::make_pair(*iter, pt2));
}
}
@@ -391,10 +394,10 @@ Transform OdometryBOW::computeTransform(
// Only add unique words
if(newSignature->getWords3().count(*iter) == 1)
{
const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
if(pcl::isFinite(pt))
const cv::Point3f & pt = newSignature->getWords3().find(*iter)->second;
if(util3d::isFinite(pt))
{
pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
cv::Point3f pt2 = util3d::transformPoint(pt, t);
localMap_.insert(std::make_pair(*iter, pt2));
}
else

172
corelib/src/OdometryF2F.cpp Normal file
View File

@@ -0,0 +1,172 @@
/*
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/EpipolarGeometry.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
namespace rtabmap {
OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomFlowKeyFrameThr()),
guessFromMotion_(Parameters::defaultOdomFlowGuessMotion()),
registration_(parameters),
motionSinceLastKeyFrame_(Transform::getIdentity())
{
Parameters::parse(parameters, Parameters::kOdomFlowKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomFlowGuessMotion(), guessFromMotion_);
}
OdometryF2F::~OdometryF2F()
{
}
void OdometryF2F::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
refFrame_ = Signature();
motionSinceLastKeyFrame_.setIdentity();
}
// return not null transform if odometry is correctly computed
Transform OdometryF2F::computeTransform(
const SensorData & data,
OdometryInfo * info)
{
UTimer timer;
Transform output;
if(!data.rightRaw().empty() && !data.stereoCameraModel().isValid())
{
UERROR("Calibrated stereo camera required");
return output;
}
if(!data.depthRaw().empty() &&
(data.cameraModels().size() != 1 || !data.cameraModels()[0].isValid()))
{
UERROR("Calibrated camera required (multi-cameras not supported).");
return output;
}
float variance = 0;
std::vector<int> inliers;
Signature newFrame(data);
if(refFrame_.getWords().size())
{
std::string rejectedMsg;
output = registration_.computeTransformationMod(
refFrame_,
newFrame,
guessFromMotion_?motionSinceLastKeyFrame_*this->previousTransform():Transform::getIdentity(),
&rejectedMsg,
&inliers,
&variance);
if(info && this->isInfoDataFilled())
{
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
EpipolarGeometry::findPairsUnique(refFrame_.getWords(), newFrame.getWords(), pairs);
info->refCorners.resize(pairs.size());
info->newCorners.resize(pairs.size());
std::map<int, int> idToIndex;
int i=0;
for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin();
iter!=pairs.end();
++iter)
{
info->refCorners[i] = iter->second.first.pt;
info->newCorners[i] = iter->second.second.pt;
idToIndex.insert(std::make_pair(iter->first, i));
++i;
}
info->cornerInliers.resize(inliers.size(), 1);
i=0;
for(; i<(int)inliers.size(); ++i)
{
info->cornerInliers[i] = idToIndex.at(inliers[i]);
}
}
}
else
{
//return Identity
output = Transform::getIdentity();
}
if(!output.isNull())
{
output = motionSinceLastKeyFrame_.inverse() * output;
motionSinceLastKeyFrame_ *= output;
// new key-frame?
if(keyFrameThr_ <= 0 || (int)inliers.size() <= keyFrameThr_)
{
UDEBUG("Update key frame");
// only generate features for the first frame
Signature newRefFrame(data);
Signature dummy;
registration_.computeTransformationMod(
newRefFrame,
dummy);
if((int)newRefFrame.getWords().size() >= this->getMinInliers())
{
refFrame_ = newRefFrame;
//reset motion
motionSinceLastKeyFrame_.setIdentity();
}
else
{
UWARN("Too low 2D corners (%d), keeping last key frame...",
(int)newRefFrame.getWords().size());
}
}
}
if(info)
{
info->type = 1;
info->variance = variance;
info->inliers = (int)inliers.size();
}
UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",
timer.elapsed(),
output.isNull()?"true":"false",
(int)inliers.size(),
(int)refFrame_.getWords().size(),
!output.isNull()?"true":"false");
return output;
}
} // namespace rtabmap

131
corelib/src/OdometryMono.cpp
View File

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d_features.h"
@@ -49,10 +50,10 @@ namespace rtabmap {
OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
Odometry(parameters),
flowWinSize_(Parameters::defaultOdomFlowWinSize()),
flowIterations_(Parameters::defaultOdomFlowIterations()),
flowEps_(Parameters::defaultOdomFlowEps()),
flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
flowWinSize_(Parameters::defaultVisCorFlowWinSize()),
flowIterations_(Parameters::defaultVisCorFlowIterations()),
flowEps_(Parameters::defaultVisCorFlowEps()),
flowMaxLevel_(Parameters::defaultVisCorFlowMaxLevel()),
localHistoryMaxSize_(Parameters::defaultOdomBowLocalHistorySize()),
initMinFlow_(Parameters::defaultOdomMonoInitMinFlow()),
initMinTranslation_(Parameters::defaultOdomMonoInitMinTranslation()),
@@ -61,10 +62,10 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
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::kVisCorFlowWinSize(), flowWinSize_);
Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), flowIterations_);
Parameters::parse(parameters, Parameters::kVisCorFlowEps(), flowEps_);
Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), flowMaxLevel_);
Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), localHistoryMaxSize_);
Parameters::parse(parameters, Parameters::kOdomMonoInitMinFlow(), initMinFlow_);
@@ -85,18 +86,18 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kKpTfIdfLikelihoodUsed(), "false"));
int nn = Parameters::defaultVisNNType();
float nndr = Parameters::defaultVisNNDR();
int nn = Parameters::defaultVisCorNNType();
float nndr = Parameters::defaultVisCorNNDR();
int featureType = Parameters::defaultVisFeatureType();
int maxFeatures = Parameters::defaultVisMaxFeatures();
Parameters::parse(parameters, Parameters::kVisNNType(), nn);
Parameters::parse(parameters, Parameters::kVisNNDR(), nndr);
Parameters::parse(parameters, Parameters::kVisCorNNType(), nn);
Parameters::parse(parameters, Parameters::kVisCorNNDR(), nndr);
Parameters::parse(parameters, Parameters::kVisFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), 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)));
customParameters.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(maxFeatures)));
int subPixWinSize = Parameters::defaultVisSubPixWinSize();
int subPixIterations = Parameters::defaultVisSubPixIterations();
@@ -354,7 +355,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
{
//PnPRansac
std::vector<int> inliersV;
cv::solvePnPRansac(
util3d::solvePnPRansac(
objectPoints,
imagePoints,
K,
@@ -364,13 +365,10 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
true,
this->getIterations(),
this->getPnPReprojError(),
#if CV_MAJOR_VERSION < 3
0, // min inliers
#else
0.99, // confidence
#endif
inliersV,
this->getPnPFlags());
this->getPnPFlags(),
this->getPnPOpenCV2());
UDEBUG("inliers=%d/%d", (int)inliersV.size(), (int)objectPoints.size());
@@ -427,15 +425,16 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
{
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(),
const std::map<int, cv::Point3f> & previousGuess = keyFrameWords3D_.find(previousS->id())->second;
std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
uMultimapToMapUnique(previousS->getWords()),
uMultimapToMapUnique(newS->getWords()),
cameraModel,
cameraTransform,
this->getIterations(),
this->getPnPReprojError(),
this->getPnPFlags(),
this->getPnPOpenCV2(),
fundMatrixReprojError_,
fundMatrixConfidence_,
previousGuess,
@@ -457,7 +456,7 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
UDEBUG("cameraTransform= %s", cameraTransform.prettyPrint().c_str());
std::multimap<int, cv::Point3f> wordsToAdd;
for(std::multimap<int, pcl::PointXYZ>::iterator iter=inliers3D.begin();
for(std::map<int, cv::Point3f>::iterator iter=inliers3D.begin();
iter != inliers3D.end();
++iter)
{
@@ -467,8 +466,8 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
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)));
cv::Point3f newPt = util3d::transformPoint(iter->second, newPose);
wordsToAdd.insert(std::make_pair(iter->first, newPt));
}
}
@@ -722,17 +721,17 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
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::Point3f> inliersRef;
std::vector<cv::Point3f> inliersRefGuess;
std::vector<cv::Point2f> imagePoints(cloud->size());
inliersRef->resize(cloud->size());
inliersRefGuess->resize(cloud->size());
inliersRef.resize(cloud->size());
inliersRefGuess.resize(cloud->size());
tmpCornersId.resize(cloud->size());
oi = 0;
UASSERT(newCorners.size() == cloud->size());
pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<cv::Point3f> newCorners3D;
if(!refDepthOrRight_.empty())
{
@@ -776,17 +775,19 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
{
imagePoints[oi] = newCorners[i];
tmpCornersId[oi] = cornerIds[i];
(*inliersRef)[oi] = cloud->at(i);
if(!newCorners3D->empty())
inliersRef[oi].x = cloud->at(i).x;
inliersRef[oi].y = cloud->at(i).y;
inliersRef[oi].z = cloud->at(i).z;
if(!newCorners3D.empty())
{
(*inliersRefGuess)[oi] = newCorners3D->at(i);
inliersRefGuess[oi] = newCorners3D.at(i);
}
++oi;
}
}
imagePoints.resize(oi);
inliersRef->resize(oi);
inliersRefGuess->resize(oi);
inliersRef.resize(oi);
inliersRefGuess.resize(oi);
tmpCornersId.resize(oi);
cornerIds = tmpCornersId;
@@ -795,25 +796,25 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
//estimate scale
float scale = 1;
std::multimap<float, float> scales; // <variance, scale>
if(!newCorners3D->empty()) // scale known
if(!newCorners3D.empty()) // scale known
{
UASSERT(inliersRefGuess->size() == inliersRef->size());
for(unsigned int i=0; i<inliersRef->size(); ++i)
UASSERT(inliersRefGuess.size() == inliersRef.size());
for(unsigned int i=0; i<inliersRef.size(); ++i)
{
if(pcl::isFinite(inliersRefGuess->at(i)))
if(util3d::isFinite(inliersRefGuess.at(i)))
{
float s = inliersRefGuess->at(i).z/inliersRef->at(i).z;
std::vector<float> errorSqrdDists(inliersRef->size());
float s = inliersRefGuess.at(i).z/inliersRef.at(i).z;
std::vector<float> errorSqrdDists(inliersRef.size());
oi = 0;
for(unsigned int j=0; j<inliersRef->size(); ++j)
for(unsigned int j=0; j<inliersRef.size(); ++j)
{
if(cloud->at(j).z>0)
{
pcl::PointXYZ refPt = inliersRef->at(j);
cv::Point3f refPt = inliersRef.at(j);
refPt.x *= s;
refPt.y *= s;
refPt.z *= s;
const pcl::PointXYZ & guess = inliersRefGuess->at(j);
const cv::Point3f & guess = inliersRefGuess.at(j);
errorSqrdDists[oi++] = uNormSquared(refPt.x-guess.x, refPt.y-guess.y, refPt.z-guess.z);
}
}
@@ -850,11 +851,19 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
}
}
else if(inliersRef->size())
else if(inliersRef.size())
{
// find centroid of the cloud and set it to 1 meter
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*inliersRef, centroid);
pcl::PointCloud<pcl::PointXYZ> inliersRefCloud;
inliersRefCloud.resize(inliersRef.size());
for(unsigned int i=0; i<inliersRef.size(); ++i)
{
inliersRefCloud[i].x = inliersRef[i].x;
inliersRefCloud[i].y = inliersRef[i].y;
inliersRefCloud[i].z = inliersRef[i].z;
}
pcl::compute3DCentroid(inliersRefCloud, centroid);
scale = 1.0f / centroid[2];
}
else
@@ -865,17 +874,17 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
if(!reject)
{
//PnPRansac
std::vector<cv::Point3f> objectPoints(inliersRef->size());
for(unsigned int i=0; i<inliersRef->size(); ++i)
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;
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(
util3d::solvePnPRansac(
objectPoints, // 3D points in ref referential
imagePoints, // 2D points in new referential
K,
@@ -885,13 +894,10 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
false,
this->getIterations(),
this->getPnPReprojError(),
#if CV_MAJOR_VERSION < 3
0, // min inliers
#else
0.99, // confidence
#endif
inliersPnP,
this->getPnPFlags());
this->getPnPFlags(),
this->getPnPOpenCV2());
UDEBUG("PnP inliers = %d / %d", (int)inliersPnP.size(), (int)objectPoints.size());
@@ -914,15 +920,16 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
std::vector<int> wordsId = uKeys(memory_->getLastWorkingSignature()->getWords());
UASSERT(wordsId.size());
UASSERT(cornerIds.size() == objectPoints.size());
std::multimap<int, pcl::PointXYZ> keyFrameWords3D;
std::map<int, cv::Point3f> keyFrameWords3D;
Transform t = this->getPose()*cameraModel.localTransform();
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()*cameraModel.localTransform());
cv::Point3f pt = util3d::transformPoint(
objectPoints.at(index),
t);
localMap_.insert(std::make_pair(id, cv::Point3f(pt.x, pt.y, pt.z)));
keyFrameWords3D.insert(std::make_pair(id, pt));
}

609
corelib/src/OdometryOpticalFlow.cpp
View File

@@ -1,609 +0,0 @@
/*
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_transforms.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_features.h"
#include "rtabmap/core/Stereo.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UMath.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/calib3d/calib3d.hpp>
namespace rtabmap {
OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomFlowKeyFrameThr()),
flowWinSize_(Parameters::defaultOdomFlowWinSize()),
flowIterations_(Parameters::defaultOdomFlowIterations()),
flowEps_(Parameters::defaultOdomFlowEps()),
flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
flowGuessFromMotion_(Parameters::defaultOdomFlowGuessMotion()),
subPixWinSize_(Parameters::defaultVisSubPixWinSize()),
subPixIterations_(Parameters::defaultVisSubPixIterations()),
subPixEps_(Parameters::defaultVisSubPixEps()),
refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>),
motionSinceLastKeyFrame_(Transform::getIdentity())
{
Parameters::parse(parameters, Parameters::kOdomFlowKeyFrameThr(), keyFrameThr_);
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::kOdomFlowGuessMotion(), flowGuessFromMotion_);
Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize_);
Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations_);
Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps_);
bool stereoOpticalFlow = Parameters::defaultStereoOpticalFlow();
Parameters::parse(parameters, Parameters::kStereoOpticalFlow(), stereoOpticalFlow);
if(stereoOpticalFlow)
{
stereo_ = new StereoOpticalFlow(parameters);
}
else
{
stereo_ = new Stereo(parameters);
}
ParametersMap::const_iterator iter;
Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultVisFeatureType();
if((iter=parameters.find(Parameters::kVisFeatureType())) != parameters.end())
{
detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
}
ParametersMap customParameters;
int maxFeatures = Parameters::defaultVisMaxFeatures();
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), 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_;
delete stereo_;
}
void OdometryOpticalFlow::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
refFrame_ = cv::Mat();
refCorners_.clear();
refCorners3D_->clear();
motionSinceLastKeyFrame_.setIdentity();
}
// return not null transform if odometry is correctly computed
Transform OdometryOpticalFlow::computeTransform(
const SensorData & data,
OdometryInfo * info)
{
UTimer timer;
Transform output;
if(!data.rightRaw().empty() && !data.stereoCameraModel().isValid())
{
UERROR("Calibrated stereo camera required");
return output;
}
if(!data.depthRaw().empty() &&
(data.cameraModels().size() != 1 || !data.cameraModels()[0].isValid()))
{
UERROR("Calibrated camera required (multi-cameras not supported).");
return output;
}
double variance = 0;
int inliers = 0;
int correspondences = 0;
if(info)
{
info->type = 1;
}
cv::Mat newLeftFrame;
// convert to grayscale
if(data.imageRaw().channels() > 1)
{
cv::cvtColor(data.imageRaw(), newLeftFrame, cv::COLOR_BGR2GRAY);
}
else
{
newLeftFrame = data.imageRaw().clone();
}
std::vector<cv::Point2f> newCorners;
UDEBUG("lastCorners_.size()=%d lastFrame_=%d depthRight=%d",
(int)refCorners_.size(), refFrame_.empty()?0:1, data.depthOrRightRaw().empty()?0:1);
if(!refFrame_.empty() &&
((data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()) || data.stereoCameraModel().isValid()) &&
refCorners_.size() &&
refCorners3D_->size())
{
UASSERT_MSG(refCorners_.size() == refCorners3D_->size(),
uFormat("%d vs %d", (int)refCorners_.size(), (int)refCorners3D_->size()).c_str());
// make guess
cv::Mat K = data.cameraModels().size()?data.cameraModels()[0].K():data.stereoCameraModel().left().K();
Transform localTransform = data.cameraModels().size()?data.cameraModels()[0].localTransform():data.stereoCameraModel().left().localTransform();
Transform guess = (motionSinceLastKeyFrame_*this->previousTransform() * localTransform).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
std::vector<cv::Point3f> objectPoints(refCorners3D_->size());
for(unsigned int i=0; i<objectPoints.size(); ++i)
{
objectPoints[i].x = refCorners3D_->at(i).x;
objectPoints[i].y = refCorners3D_->at(i).y;
objectPoints[i].z = refCorners3D_->at(i).z;
}
if(flowGuessFromMotion_ && !(motionSinceLastKeyFrame_*this->previousTransform()).isIdentity())
{
UDEBUG("project points to new image");
cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), newCorners);
}
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
int winSize = flowWinSize_;
cv::calcOpticalFlowPyrLK(
refFrame_,
newLeftFrame,
refCorners_,
newCorners,
status,
err,
cv::Size(winSize, winSize),
(newCorners.size()||!flowGuessFromMotion_)?flowMaxLevel_:3,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (newCorners.size()?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3DKept(new pcl::PointCloud<pcl::PointXYZ>);
refCorners3DKept->resize(status.size());
std::vector<cv::Point3f> objectPointsKept(status.size());
std::vector<cv::Point2f> refCornersKept(status.size());
std::vector<cv::Point2f> newCornersKept(status.size());
int ki = 0;
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] &&
uIsInBounds(newCorners[i].x, 0.0f, float(data.depthOrRightRaw().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depthOrRightRaw().rows)))
{
refCorners3DKept->at(ki) = refCorners3D_->at(i);
objectPointsKept[ki] = objectPoints[i];
refCornersKept[ki] = refCorners_[i];
newCornersKept[ki] = newCorners[i];
++ki;
}
}
refCorners3DKept->resize(ki);
objectPointsKept.resize(ki);
refCornersKept.resize(ki);
newCornersKept.resize(ki);
correspondences = ki;
if(correspondences && correspondences >= this->getMinInliers())
{
// get new 3D points
pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3DKept;
if(!isVarianceFromInliersCount() || this->getEstimationType() != 1)
{
// Don't compute the new 3D points if the variance is not required on PnP estimation
if(!data.rightRaw().empty())
{
// stereo
std::vector<unsigned char> stereoStatus;
std::vector<cv::Point2f> rightCorners;
rightCorners = stereo_->computeCorrespondences(
newLeftFrame,
data.rightRaw(),
newCornersKept,
stereoStatus);
if(this->getMaxDepth() > 0.0f)
{
UASSERT(status.size() == newCornersKept.size() && status.size() == rightCorners.size());
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] != 0)
{
float d = data.stereoCameraModel().computeDepth(newCornersKept[i].x - rightCorners[i].x);
if(this->getMaxDepth() > 0.0f && d > this->getMaxDepth())
{
status[i] = 0;
}
}
}
}
newCorners3DKept = util3d::generateKeypoints3DStereo(
newCornersKept,
rightCorners,
data.stereoCameraModel(),
stereoStatus);
}
else
{
//depth
std::vector<cv::KeyPoint> newCornersKeptKpt;
cv::KeyPoint::convert(newCornersKept, newCornersKeptKpt);
newCorners3DKept = util3d::generateKeypoints3DDepth(
newCornersKeptKpt,
data.depthRaw(),
data.cameraModels());
}
UASSERT(newCorners3DKept.get() != 0);
}
std::vector<int> inliersV;
if(this->getEstimationType() == 1) // PnP
{
if(this->isInfoDataFilled() && info)
{
info->refCorners = refCornersKept;
info->newCorners = newCornersKept;
}
//PnPRansac
cv::solvePnPRansac(
objectPointsKept,
newCornersKept,
K,
cv::Mat(),
rvec,
tvec,
true,
this->getIterations(),
this->getPnPReprojError(),
#if CV_MAJOR_VERSION < 3
0, // min inliers
#else
0.99, // confidence
#endif
inliersV,
this->getPnPFlags());
cv::Rodrigues(rvec, R);
Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
inliers = (int)inliersV.size();
if((int)inliersV.size() >= this->getMinInliers())
{
// make it incremental
output = (localTransform * pnp).inverse();
// compute variance from 3D correspondences error
variance = 1;
if(!isVarianceFromInliersCount())
{
UASSERT(objectPointsKept.size() == newCorners3DKept->size());
std::vector<float> errorSqrdDists(inliersV.size());
int oi = 0;
for(unsigned int i=0; i<inliersV.size(); ++i)
{
if(pcl::isFinite(newCorners3DKept->at(inliersV[i])))
{
const cv::Point3f & objPt = objectPointsKept[inliersV[i]];
pcl::PointXYZ newPt = util3d::transformPoint(newCorners3DKept->at(inliersV[i]), output);
errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
}
}
errorSqrdDists.resize(oi);
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
{
// Get 3D correspondences (remove NaN)
pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesRef(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
correspondencesRef->resize(newCornersKept.size());
correspondencesNew->resize(newCornersKept.size());
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(newCornersKept.size());
info->newCorners.resize(newCornersKept.size());
}
int oi = 0;
UASSERT(newCorners3DKept->size() == newCornersKept.size());
for(unsigned int i=0; i<newCornersKept.size(); ++i)
{
if(pcl::isFinite(newCorners3DKept->at(i)) &&
(this->getMaxDepth() == 0.0f || newCorners3DKept->at(i).z < this->getMaxDepth()))
{
//Add 3D correspondences!
correspondencesRef->at(oi) = refCorners3DKept->at(i);
correspondencesNew->at(oi) = newCorners3DKept->at(i);
if(this->isInfoDataFilled() && info)
{
info->refCorners[oi] = refCornersKept[i];
info->newCorners[oi] = newCornersKept[i];
}
++oi;
}
}
correspondencesRef->resize(oi);
correspondencesNew->resize(oi);
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(oi);
info->newCorners.resize(oi);
}
correspondences = oi;
UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)newCornersKept.size());
if(correspondences >= this->getMinInliers())
{
UTimer timerRANSAC;
Transform t = util3d::transformFromXYZCorrespondences(
correspondencesNew,
correspondencesRef,
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);
}
}
else
{
UWARN("Not enough correspondences (%d)", correspondences);
}
}
if(this->isInfoDataFilled() && info)
{
info->cornerInliers = inliersV;
}
}
}
else
{
//return Identity
output = Transform::getIdentity();
}
newCorners.clear();
if(!output.isNull())
{
output = motionSinceLastKeyFrame_.inverse() * output;
// new key-frame?
if(keyFrameThr_ <= 0 || inliers <= keyFrameThr_)
{
// Copy or generate new keypoints
if(data.keypoints().size())
{
cv::KeyPoint::convert(data.keypoints(), newCorners);
}
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())
{
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;
UTimer corner3dTimer;
if(!data.rightRaw().empty())
{
// stereo
std::vector<unsigned char> stereoStatus;
std::vector<cv::Point2f> rightCorners;
rightCorners = stereo_->computeCorrespondences(
newLeftFrame,
data.rightRaw(),
newCorners,
stereoStatus);
pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3DTmp = util3d::generateKeypoints3DStereo(
newCorners,
rightCorners,
data.stereoCameraModel(),
stereoStatus);
UASSERT(refCorners3DTmp->size() == newCorners.size());
for(unsigned int i=0; i<newCorners.size(); ++i)
{
if(pcl::isFinite(refCorners3DTmp->at(i)) &&
(this->getMaxDepth() <= 0.0f || data.stereoCameraModel().computeDepth(newCorners[i].x - rightCorners[i].x) <= this->getMaxDepth() ))
{
newCorners3D->at(oi) = refCorners3DTmp->at(i);
newCornersFiltered[oi] = newCorners[i];
++oi;
}
}
}
else
{
// depth
for(unsigned int i=0; i<newCorners.size(); ++i)
{
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depthRaw().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depthRaw().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(
data.depthRaw(),
newCorners[i].x,
newCorners[i].y,
data.cameraModels()[0].cx(),
data.cameraModels()[0].cy(),
data.cameraModels()[0].fx(),
data.cameraModels()[0].fy(),
true);
if(pcl::isFinite(pt) &&
pt.z > 0 &&
(this->getMaxDepth() == 0.0f || pt.z < this->getMaxDepth()))
{
newCorners3D->at(oi) = util3d::transformPoint(pt, data.cameraModels()[0].localTransform());
newCornersFiltered[oi] = newCorners[i];
++oi;
}
}
}
}
UDEBUG("Computing 3d corners = %f s", corner3dTimer.ticks());
newCornersFiltered.resize(oi);
newCorners3D->resize(oi);
if((int)newCornersFiltered.size() >= this->getMinInliers())
{
refFrame_ = newLeftFrame;
refCorners_ = newCornersFiltered;
refCorners3D_ = newCorners3D;
//reset motion
motionSinceLastKeyFrame_.setIdentity();
}
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();
}
}
else
{
motionSinceLastKeyFrame_ *= output;
}
}
if(info)
{
info->type = 1;
info->variance = variance;
info->inliers = inliers;
info->features = (int)refCorners_.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;
}
} // namespace rtabmap

24
corelib/src/Parameters.cpp
View File

@@ -141,6 +141,8 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
// removed parameters
// 0.11.0
removedParameters_.insert(std::make_pair("Kp/WordsPerImage", std::make_pair(true, Parameters::kKpMaxFeatures())));
removedParameters_.insert(std::make_pair("Mem/LaserScanVoxelSize", std::make_pair(false, Parameters::kMemLaserScanDownsampleStepSize())));
removedParameters_.insert(std::make_pair("Mem/LocalSpaceLinksKeptInWM", std::make_pair(false, "")));
@@ -161,8 +163,13 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("Odom/PnPReprojError", std::make_pair(true, Parameters::kVisPnPReprojError())));
removedParameters_.insert(std::make_pair("Odom/PnPFlags", std::make_pair(true, Parameters::kVisPnPFlags())));
removedParameters_.insert(std::make_pair("OdomBow/NNType", std::make_pair(true, Parameters::kVisNNType())));
removedParameters_.insert(std::make_pair("OdomBow/NNDR", std::make_pair(true, Parameters::kVisNNDR())));
removedParameters_.insert(std::make_pair("OdomBow/NNType", std::make_pair(true, Parameters::kVisCorNNType())));
removedParameters_.insert(std::make_pair("OdomBow/NNDR", std::make_pair(true, Parameters::kVisCorNNDR())));
removedParameters_.insert(std::make_pair("OdomFlow/WinSize", std::make_pair(true, Parameters::kVisCorFlowWinSize())));
removedParameters_.insert(std::make_pair("OdomFlow/Iterations", std::make_pair(true, Parameters::kVisCorFlowIterations())));
removedParameters_.insert(std::make_pair("OdomFlow/Eps", std::make_pair(true, Parameters::kVisCorFlowEps())));
removedParameters_.insert(std::make_pair("OdomFlow/MaxLevel", std::make_pair(true, Parameters::kVisCorFlowMaxLevel())));
removedParameters_.insert(std::make_pair("OdomSubPix/WinSize", std::make_pair(true, Parameters::kVisSubPixWinSize())));
removedParameters_.insert(std::make_pair("OdomSubPix/Iterations", std::make_pair(true, Parameters::kVisSubPixIterations())));
@@ -173,8 +180,8 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("LccReextract/MaxWords", std::make_pair(false, Parameters::kVisMaxFeatures())));
removedParameters_.insert(std::make_pair("LccReextract/MaxDepth", std::make_pair(false, Parameters::kVisMaxDepth())));
removedParameters_.insert(std::make_pair("LccReextract/RoiRatios", std::make_pair(false, Parameters::kVisRoiRatios())));
removedParameters_.insert(std::make_pair("LccReextract/NNType", std::make_pair(false, Parameters::kVisNNType())));
removedParameters_.insert(std::make_pair("LccReextract/NNDR", std::make_pair(false, Parameters::kVisNNDR())));
removedParameters_.insert(std::make_pair("LccReextract/NNType", std::make_pair(false, Parameters::kVisCorNNType())));
removedParameters_.insert(std::make_pair("LccReextract/NNDR", std::make_pair(false, Parameters::kVisCorNNDR())));
removedParameters_.insert(std::make_pair("LccBow/EstimationType", std::make_pair(false, Parameters::kVisEstimationType())));
removedParameters_.insert(std::make_pair("LccBow/InlierDistance", std::make_pair(false, Parameters::kVisInlierDistance())));
@@ -235,8 +242,8 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("Odom/Type", std::make_pair(true, Parameters::kVisFeatureType())));
removedParameters_.insert(std::make_pair("Odom/MaxWords", std::make_pair(true, Parameters::kVisMaxFeatures())));
removedParameters_.insert(std::make_pair("Odom/LocalHistory", std::make_pair(true, Parameters::kOdomBowLocalHistorySize())));
removedParameters_.insert(std::make_pair("Odom/NearestNeighbor", std::make_pair(true, Parameters::kVisNNType())));
removedParameters_.insert(std::make_pair("Odom/NNDR", std::make_pair(true, Parameters::kVisNNDR())));
removedParameters_.insert(std::make_pair("Odom/NearestNeighbor", std::make_pair(true, Parameters::kVisCorNNType())));
removedParameters_.insert(std::make_pair("Odom/NNDR", std::make_pair(true, Parameters::kVisCorNNDR())));
}
return removedParameters_;
}
@@ -420,7 +427,10 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
}
uInsert(parameters, ParametersPair(key, iter->second));
if(Parameters::getDefaultParameters().find(key) != Parameters::getDefaultParameters().end())
{
uInsert(parameters, ParametersPair(key, iter->second));
}
}
}
}

16
corelib/src/RegistrationIcp.cpp
View File

@@ -77,14 +77,14 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
UASSERT_MSG(_pointToPlaneNormalNeighbors > 0, uFormat("value=%d", _pointToPlaneNormalNeighbors).c_str());
}
Transform RegistrationIcp::computeTransformation(
const Signature & fromSignature,
const Signature & toSignature,
Transform RegistrationIcp::computeTransformationMod(
Signature & fromSignature,
Signature & toSignature,
Transform guess,
std::string * rejectedMsg,
int * inliersOut,
std::vector<int> * inliersOut,
float * varianceOut,
float * inliersRatioOut)
float * inliersRatioOut) const
{
return computeTransformation(
fromSignature.sensorData(),
@@ -101,9 +101,9 @@ Transform RegistrationIcp::computeTransformation(
const SensorData & dataTo,
Transform guess,
std::string * rejectedMsg,
int * inliersOut,
std::vector<int> * inliersOut,
float * varianceOut,
float * inliersRatioOut)
float * inliersRatioOut) const
{
UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
UDEBUG("Voxel size=%f", _voxelSize);
@@ -298,7 +298,7 @@ Transform RegistrationIcp::computeTransformation(
}
if(inliersOut)
{
*inliersOut = correspondences;
inliersOut->push_back(correspondences);
}
if(inliersRatioOut)
{

775
corelib/src/RegistrationVis.cpp
View File

@@ -30,11 +30,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/RegistrationVis.h>
#include <rtabmap/core/util3d_motion_estimation.h>
#include <rtabmap/core/util3d_features.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/VWDictionary.h>
#include <rtabmap/core/Features2d.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UMath.h>
namespace rtabmap {
@@ -46,8 +49,15 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
_force2D(Parameters::defaultVisForce2D()),
_epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
_estimationType(Parameters::defaultVisEstimationType()),
_forwardEstimateOnly(Parameters::defaultVisForwardEstOnly()),
_PnPReprojError(Parameters::defaultVisPnPReprojError()),
_PnPFlags(Parameters::defaultVisPnPFlags())
_PnPFlags(Parameters::defaultVisPnPFlags()),
_PnPOpenCV2(Parameters::defaultVisPnPOpenCV2()),
_correspondencesApproach(Parameters::defaultVisCorType()),
_flowWinSize(Parameters::defaultVisCorFlowWinSize()),
_flowIterations(Parameters::defaultVisCorFlowIterations()),
_flowEps(Parameters::defaultVisCorFlowEps()),
_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel())
{
_featureParameters = Parameters::getDefaultParameters();
uInsert(_featureParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
@@ -59,10 +69,10 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
uInsert(_featureParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
uInsert(_featureParameters, ParametersPair(Parameters::kMemGenerateIds(), "true"));
uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisNNType())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), _featureParameters.at(Parameters::kVisNNDR())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisCorNNType())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), _featureParameters.at(Parameters::kVisCorNNDR())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), _featureParameters.at(Parameters::kVisFeatureType())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpWordsPerImage(), _featureParameters.at(Parameters::kVisMaxFeatures())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), _featureParameters.at(Parameters::kVisMaxFeatures())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), _featureParameters.at(Parameters::kVisMaxDepth())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), _featureParameters.at(Parameters::kVisMinDepth())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), _featureParameters.at(Parameters::kVisRoiRatios())));
@@ -86,6 +96,12 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
Parameters::parse(parameters, Parameters::kVisPnPOpenCV2(), _PnPOpenCV2);
Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
Parameters::parse(parameters, Parameters::kVisCorFlowEps(), _flowEps);
Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
UASSERT_MSG(_inlierDistance > 0.0f, uFormat("value=%f", _inlierDistance).c_str());
@@ -101,13 +117,13 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
}
}
if(uContains(parameters, Parameters::kVisNNType()))
if(uContains(parameters, Parameters::kVisCorNNType()))
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kVisNNType())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kVisCorNNType())));
}
if(uContains(parameters, Parameters::kVisNNDR()))
if(uContains(parameters, Parameters::kVisCorNNDR()))
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kVisNNDR())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kVisCorNNDR())));
}
if(uContains(parameters, Parameters::kVisFeatureType()))
{
@@ -115,7 +131,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
}
if(uContains(parameters, Parameters::kVisMaxFeatures()))
{
uInsert(_featureParameters, ParametersPair(Parameters::kKpWordsPerImage(), parameters.at(Parameters::kVisMaxFeatures())));
uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
}
if(uContains(parameters, Parameters::kVisMaxDepth()))
{
@@ -147,235 +163,593 @@ RegistrationVis::~RegistrationVis()
{
}
Transform RegistrationVis::computeTransformation(
const Signature & fromSignature,
const Signature & toSignature,
Transform guess, // guess is ignored for RegistrationVis
Transform RegistrationVis::computeTransformationMod(
Signature & fromSignature,
Signature & toSignature,
Transform guess, // guess is only used by Optical Flow correspondences (flowMaxLevel is set to 0 when guess is used)
std::string * rejectedMsg,
int * inliersOut,
std::vector<int> * inliersOut,
float * varianceOut,
float * inliersRatioOut)
float * inliersRatioOut) const
{
Transform transform;
UDEBUG("%s=%d", Parameters::kVisMinInliers().c_str(), _minInliers);
UDEBUG("%s=%f", Parameters::kVisInlierDistance().c_str(), _inlierDistance);
UDEBUG("%s=%d", Parameters::kVisIterations().c_str(), _iterations);
UDEBUG("%s=%d", Parameters::kVisForce2D().c_str(), _force2D?1:0);
UDEBUG("%s=%d", Parameters::kVisEstimationType().c_str(), _estimationType);
UDEBUG("%s=%d", Parameters::kVisForwardEstOnly().c_str(), _forwardEstimateOnly);
UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach?1:0);
UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
UDEBUG("%s=%f", Parameters::kVisCorFlowEps().c_str(), _flowEps);
UDEBUG("%s=%d", Parameters::kVisCorFlowMaxLevel().c_str(), _flowMaxLevel);
UDEBUG("Input(%d): from=%d words, %d 3D words, %d kpts, %d descriptors",
fromSignature.id(),
(int)fromSignature.getWords().size(),
(int)fromSignature.getWords3().size(),
(int)fromSignature.sensorData().keypoints().size(),
fromSignature.sensorData().descriptors().rows);
UDEBUG("Input(%d): to=%d words, %d 3D words, %d kpts, %d descriptors",
toSignature.id(),
(int)toSignature.getWords().size(),
(int)toSignature.getWords3().size(),
(int)toSignature.sensorData().keypoints().size(),
toSignature.sensorData().descriptors().rows);
std::string msg;
// Guess transform from visual words
int inliersCount= 0;
double variance = 1.0;
// Extract features?
const std::multimap<int, cv::KeyPoint> * wordsFrom = 0;
const std::multimap<int, cv::KeyPoint> * wordsTo = 0;
const std::multimap<int, pcl::PointXYZ> * words3From = 0;
const std::multimap<int, pcl::PointXYZ> * words3To = 0;
std::multimap<int, cv::KeyPoint> extractedWordsFrom, extractedWordsTo;
std::multimap<int, pcl::PointXYZ> extractedWords3From, extractedWords3To;
if(fromSignature.getWords().size() == 0 && toSignature.getWords().size() == 0)
////////////////////
// Find correspondences
////////////////////
if(fromSignature.getWords().size() && fromSignature.getWords3().size() &&
toSignature.getWords().size() && (_estimationType==1 || toSignature.getWords3().size()))
{
// Use the Memory class to extract features
Memory memory(_featureParameters);
// Add signatures
SensorData dataFrom = fromSignature.sensorData();
SensorData dataTo = toSignature.sensorData();
// make sure there are no features already in the SensorData
dataFrom.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
dataTo.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
UTimer timeT;
memory.update(dataFrom);
if(memory.getLastWorkingSignature() == 0)
{
UWARN("Failed to extract features for node %d", dataFrom.id());
}
else
{
extractedWordsFrom = memory.getLastWorkingSignature()->getWords();
extractedWords3From = memory.getLastWorkingSignature()->getWords3();
UDEBUG("timeTo = %fs", timeT.ticks());
memory.update(dataTo);
if(memory.getLastWorkingSignature() == 0)
{
UWARN("Failed to extract features for node %d", dataTo.id());
}
else
{
extractedWordsTo = memory.getLastWorkingSignature()->getWords();
extractedWords3To = memory.getLastWorkingSignature()->getWords3();
UDEBUG("timeFrom = %fs", timeT.ticks());
}
}
wordsFrom = &extractedWordsFrom;
wordsTo = &extractedWordsTo;
words3From = &extractedWords3From;
words3To = &extractedWords3To;
// no need to extract new features, we have all the data we need
UDEBUG("");
}
else
{
wordsFrom = &fromSignature.getWords();
wordsTo = &toSignature.getWords();
words3From = &fromSignature.getWords3();
words3To = &toSignature.getWords3();
UDEBUG("");
// just some checks to make sure that input data are ok
UASSERT((fromSignature.getWords().empty() && fromSignature.getWords3().empty())||
(fromSignature.getWords().size() == fromSignature.getWords3().size()));
UASSERT(fromSignature.sensorData().keypoints().size() == fromSignature.sensorData().descriptors().rows ||
fromSignature.sensorData().descriptors().rows == 0);
UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
(toSignature.getWords().size() == toSignature.getWords3().size()));
UASSERT(toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
toSignature.sensorData().descriptors().rows == 0);
UASSERT(fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
fromSignature.sensorData().imageRaw().type() == CV_8UC3);
UASSERT(toSignature.sensorData().imageRaw().type() == CV_8UC1 ||
toSignature.sensorData().imageRaw().type() == CV_8UC3);
Feature2D * detector = Feature2D::create(_featureParameters);
std::vector<cv::KeyPoint> kptsFrom;
if(fromSignature.getWords().empty())
{
if(fromSignature.sensorData().keypoints().empty())
{
if(fromSignature.sensorData().imageRaw().channels() > 1)
{
cv::Mat tmp;
cv::cvtColor(fromSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
fromSignature.sensorData().setImageRaw(tmp);
}
kptsFrom = detector->generateKeypoints(fromSignature.sensorData().imageRaw());
}
else
{
kptsFrom = fromSignature.sensorData().keypoints();
}
}
else
{
kptsFrom = uValues(fromSignature.getWords());
}
std::multimap<int, cv::KeyPoint> wordsFrom;
std::multimap<int, cv::KeyPoint> wordsTo;
std::multimap<int, cv::Point3f> words3From;
std::multimap<int, cv::Point3f> words3To;
if(_correspondencesApproach == 1) //Optical Flow
{
UDEBUG("");
// convert to grayscale
if(fromSignature.sensorData().imageRaw().channels() > 1)
{
cv::Mat tmp;
cv::cvtColor(fromSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
fromSignature.sensorData().setImageRaw(tmp);
}
if(toSignature.sensorData().imageRaw().channels() > 1)
{
cv::Mat tmp;
cv::cvtColor(toSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
toSignature.sensorData().setImageRaw(tmp);
}
std::vector<cv::Point3f> kptsFrom3D;
if(fromSignature.getWords3().empty())
{
kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
}
else
{
kptsFrom3D = uValues(fromSignature.getWords3());
}
if(!toSignature.sensorData().imageRaw().empty())
{
std::vector<cv::Point2f> cornersFrom;
cv::KeyPoint::convert(kptsFrom, cornersFrom);
std::vector<cv::Point2f> cornersTo;
bool guessSet = !guess.isIdentity() && !guess.isNull();
if(guessSet)
{
Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModel().left().localTransform();
Transform guessCameraRef = (guess * localTransform).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModel().left().K();
cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
}
// Find features in the new left image
UDEBUG("guessSet = %d", guessSet?1:0);
std::vector<unsigned char> status;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
int winSize = _flowWinSize;
cv::calcOpticalFlowPyrLK(
fromSignature.sensorData().imageRaw(),
toSignature.sensorData().imageRaw(),
cornersFrom,
cornersTo,
status,
err,
cv::Size(winSize, winSize),
guessSet?0:_flowMaxLevel,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, _flowIterations, _flowEps),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
UASSERT(kptsFrom.size() == kptsFrom3D.size());
std::vector<cv::KeyPoint> kptsTo(kptsFrom.size());
std::vector<cv::Point3f> kptsFrom3DKept(kptsFrom3D.size());
int ki = 0;
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] &&
uIsInBounds(cornersTo[i].x, 0.0f, float(toSignature.sensorData().depthOrRightRaw().cols)) &&
uIsInBounds(cornersTo[i].y, 0.0f, float(toSignature.sensorData().depthOrRightRaw().rows)))
{
kptsFrom[ki] = cv::KeyPoint(cornersFrom[i], 1);
kptsFrom3DKept[ki] = kptsFrom3D[i];
kptsTo[ki++] = cv::KeyPoint(cornersTo[i], 1);
}
}
kptsFrom.resize(ki);
kptsTo.resize(ki);
kptsFrom3DKept.resize(ki);
std::vector<cv::Point3f> kptsTo3D;
if(_estimationType == 0 || (_estimationType == 1 && !_varianceFromInliersCount) || !_forwardEstimateOnly)
{
kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
}
UASSERT(kptsFrom.size() == kptsFrom3DKept.size());
UASSERT(kptsFrom.size() == kptsTo.size());
UASSERT(kptsTo3D.size() == 0 || kptsTo.size() == kptsTo3D.size());
for(unsigned int i=0; i< kptsFrom3DKept.size(); ++i)
{
wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
words3From.insert(std::make_pair(i, kptsFrom3DKept[i]));
wordsTo.insert(std::make_pair(i, kptsTo[i]));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(i, kptsTo3D[i]));
}
}
toSignature.sensorData().setFeatures(kptsTo, cv::Mat());
}
else
{
UASSERT(kptsFrom.size() == kptsFrom3D.size());
for(unsigned int i=0; i< kptsFrom3D.size(); ++i)
{
if(util3d::isFinite(kptsFrom3D[i]))
{
wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
words3From.insert(std::make_pair(i, kptsFrom3D[i]));
}
}
toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
}
fromSignature.sensorData().setFeatures(kptsFrom, cv::Mat());
}
else // Features Matching
{
UDEBUG("");
std::vector<cv::KeyPoint> kptsTo;
if(toSignature.getWords().empty())
{
if(toSignature.sensorData().keypoints().empty() &&
!toSignature.sensorData().imageRaw().empty())
{
if(toSignature.sensorData().imageRaw().channels() > 1)
{
cv::Mat tmp;
cv::cvtColor(toSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
toSignature.sensorData().setImageRaw(tmp);
}
kptsTo = detector->generateKeypoints(toSignature.sensorData().imageRaw());
}
else
{
kptsTo = toSignature.sensorData().keypoints();
}
}
else
{
kptsTo = uValues(toSignature.getWords());
}
// extract descriptors
UDEBUG("kptsFrom=%d", (int)kptsFrom.size());
UDEBUG("kptsTo=%d", (int)kptsTo.size());
cv::Mat descriptorsFrom;
cv::Mat descriptorsTo;
if(fromSignature.getWords().empty() && fromSignature.sensorData().descriptors().rows == kptsFrom.size())
{
descriptorsFrom = fromSignature.sensorData().descriptors();
}
else
{
descriptorsFrom = detector->generateDescriptors(fromSignature.sensorData().imageRaw(), kptsFrom);
}
if(toSignature.getWords().empty() && toSignature.sensorData().descriptors().rows == kptsTo.size())
{
descriptorsTo = toSignature.sensorData().descriptors();
}
else if(!toSignature.sensorData().imageRaw().empty())
{
descriptorsTo = detector->generateDescriptors(toSignature.sensorData().imageRaw(), kptsTo);
}
// create 3D keypoints
std::vector<cv::Point3f> kptsFrom3D;
std::vector<cv::Point3f> kptsTo3D;
if(fromSignature.getWords3().empty())
{
kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
}
else
{
kptsFrom3D = uValues(fromSignature.getWords3());
}
if((_estimationType == 0 || (_estimationType == 1 && !_varianceFromInliersCount) || !_forwardEstimateOnly) &&
toSignature.getWords3().empty() &&
!toSignature.sensorData().imageRaw().empty())
{
kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
}
else
{
kptsTo3D = uValues(toSignature.getWords3());
}
// We have all data we need here, so match using the vocabulary
UDEBUG("descriptorsFrom=%d", descriptorsFrom.rows);
VWDictionary dictionary(_featureParameters);
std::list<int> fromWordIds = dictionary.addNewWords(descriptorsFrom, 1);
std::list<int> toWordIds;
UDEBUG("descriptorsTo=%d", descriptorsTo.rows);
if(descriptorsTo.rows)
{
dictionary.update();
toWordIds = dictionary.addNewWords(descriptorsTo, 2);
}
dictionary.clear(false);
std::multiset<int> fromWordIdsSet(fromWordIds.begin(), fromWordIds.end());
std::multiset<int> toWordIdsSet(toWordIds.begin(), toWordIds.end());
UASSERT(kptsFrom3D.size() == kptsFrom.size());
UASSERT(fromWordIds.size() == kptsFrom.size());
int i=0;
for(std::list<int>::iterator iter=fromWordIds.begin(); iter!=fromWordIds.end(); ++iter)
{
if(fromWordIdsSet.count(*iter) == 1)
{
wordsFrom.insert(std::make_pair(*iter, kptsFrom[i]));
words3From.insert(std::make_pair(*iter, kptsFrom3D[i]));
}
++i;
}
UASSERT(kptsTo3D.size() == 0 || kptsTo3D.size() == kptsTo.size());
UASSERT(toWordIds.size() == kptsTo.size());
i=0;
for(std::list<int>::iterator iter=toWordIds.begin(); iter!=toWordIds.end(); ++iter)
{
if(toWordIdsSet.count(*iter) == 1)
{
wordsTo.insert(std::make_pair(*iter, kptsTo[i]));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(*iter, kptsTo3D[i]));
}
}
++i;
}
//remove doubles
fromSignature.sensorData().setFeatures(kptsFrom, descriptorsFrom);
toSignature.sensorData().setFeatures(kptsTo, descriptorsTo);
}
fromSignature.setWords(wordsFrom);
fromSignature.setWords3(words3From);
toSignature.setWords(wordsTo);
toSignature.setWords3(words3To);
delete detector;
}
if(_estimationType == 2) // Epipolar Geometry
/////////////////////
// Motion estimation
/////////////////////
Transform transform;
float variance = 1.0f;
if(toSignature.getWords().size() || !toSignature.sensorData().imageRaw().empty())
{
if(!toSignature.sensorData().stereoCameraModel().isValid() &&
(toSignature.sensorData().cameraModels().size() != 1 ||
!toSignature.sensorData().cameraModels()[0].isValid()))
Transform transforms[2];
std::vector<int> inliers[2];
double variances[2] = {1.0f};
for(int dir=0; dir<(!_forwardEstimateOnly?2:1); ++dir)
{
UERROR("Calibrated camera required (multi-cameras not supported).");
}
else if((int)wordsFrom->size() >= _minInliers &&
(int)wordsTo->size() >= _minInliers)
{
UASSERT(fromSignature.sensorData().stereoCameraModel().isValid() || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = fromSignature.sensorData().stereoCameraModel().isValid()?fromSignature.sensorData().stereoCameraModel().left():fromSignature.sensorData().cameraModels()[0];
// we only need the camera transform, send guess words3 for scale estimation
Transform cameraTransform;
std::multimap<int, pcl::PointXYZ> inliers3D = util3d::generateWords3DMono(
*wordsFrom,
*wordsTo,
cameraModel,
cameraTransform,
_iterations,
_PnPReprojError,
_PnPFlags, // cv::SOLVEPNP_ITERATIVE
1.0f,
0.99f,
*words3From, // for scale estimation
&variance);
inliersCount = (int)inliers3D.size();
if(!cameraTransform.isNull())
// A to B
const Signature * signatureA;
const Signature * signatureB;
if(dir == 0)
{
if((int)inliers3D.size() >= _minInliers)
signatureA = &fromSignature;
signatureB = &toSignature;
}
else
{
signatureA = &toSignature;
signatureB = &fromSignature;
}
if(_estimationType == 2) // Epipolar Geometry
{
UDEBUG("");
if(!signatureB->sensorData().stereoCameraModel().isValid() &&
(signatureB->sensorData().cameraModels().size() != 1 ||
!signatureB->sensorData().cameraModels()[0].isValid()))
{
if(variance <= _epipolarGeometryVar)
UERROR("Calibrated camera required (multi-cameras not supported).");
}
else if((int)signatureA->getWords().size() >= _minInliers &&
(int)signatureB->getWords().size() >= _minInliers)
{
UASSERT(signatureA->sensorData().stereoCameraModel().isValid() || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModel().isValid()?signatureA->sensorData().stereoCameraModel().left():signatureA->sensorData().cameraModels()[0];
// we only need the camera transform, send guess words3 for scale estimation
Transform cameraTransform;
std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
uMultimapToMapUnique(signatureA->getWords()),
uMultimapToMapUnique(signatureB->getWords()),
cameraModel,
cameraTransform,
_iterations,
_PnPReprojError,
_PnPFlags, // cv::SOLVEPNP_ITERATIVE
_PnPOpenCV2,
1.0f,
0.99f,
uMultimapToMapUnique(signatureA->getWords3()), // for scale estimation
&variances[dir]);
inliers[dir] = uKeys(inliers3D);
if(!cameraTransform.isNull())
{
transform = cameraTransform;
if((int)inliers3D.size() >= _minInliers)
{
if(variances[dir] <= _epipolarGeometryVar)
{
transforms[dir] = cameraTransform;
}
else
{
msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _epipolarGeometryVar, variances[dir]);
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _epipolarGeometryVar, variance);
msg = uFormat("No camera transform found");
UINFO(msg.c_str());
}
}
else if(signatureA->getWords().size() == 0)
{
msg = uFormat("No enough features (%d)", (int)signatureA->getWords().size());
UWARN(msg.c_str());
}
else
{
msg = uFormat("No camera model");
UWARN(msg.c_str());
}
}
else if(_estimationType == 1) // PnP
{
UDEBUG("");
if(!signatureB->sensorData().stereoCameraModel().isValid() &&
(signatureB->sensorData().cameraModels().size() != 1 ||
!signatureB->sensorData().cameraModels()[0].isValid()))
{
UERROR("Calibrated camera required (multi-cameras not supported). Id=%d Models=%d StereoModel=%d weight=%d",
signatureB->id(),
(int)signatureB->sensorData().cameraModels().size(),
signatureB->sensorData().stereoCameraModel().isValid()?1:0,
signatureB->getWeight());
}
else
{
UDEBUG("words from3D=%d to2D=%d", (int)signatureA->getWords3().size(), (int)signatureB->getWords().size());
// 3D to 2D
if((int)signatureA->getWords3().size() >= _minInliers &&
(int)signatureB->getWords().size() >= _minInliers)
{
UASSERT(signatureB->sensorData().stereoCameraModel().isValid() || (signatureB->sensorData().cameraModels().size() == 1 && signatureB->sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = signatureB->sensorData().stereoCameraModel().isValid()?signatureB->sensorData().stereoCameraModel().left():signatureB->sensorData().cameraModels()[0];
std::vector<int> inliersV;
transforms[dir] = util3d::estimateMotion3DTo2D(
uMultimapToMapUnique(signatureA->getWords3()),
uMultimapToMapUnique(signatureB->getWords()),
cameraModel,
_minInliers,
_iterations,
_PnPReprojError,
_PnPFlags,
_PnPOpenCV2,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
uMultimapToMapUnique(signatureA->getWords3()),
_varianceFromInliersCount?0:&variances[dir],
0,
&inliersV);
inliers[dir] = inliersV;
if(transforms[dir].isNull())
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
(int)inliers[dir].size(), _minInliers, signatureA->id(), signatureB->id());
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
(int)signatureA->getWords3().size(), (int)signatureB->getWords().size(), _minInliers);
UINFO(msg.c_str());
}
}
}
else
{
UDEBUG("");
// 3D -> 3D
if((int)signatureA->getWords3().size() >= _minInliers &&
(int)signatureB->getWords3().size() >= _minInliers)
{
std::vector<int> inliersV;
transforms[dir] = util3d::estimateMotion3DTo3D(
uMultimapToMapUnique(signatureA->getWords3()),
uMultimapToMapUnique(signatureB->getWords3()),
_minInliers,
_inlierDistance,
_iterations,
_refineIterations,
&variances[dir],
0,
&inliersV);
inliers[dir] = inliersV;
if(transforms[dir].isNull())
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
(int)inliers[dir].size(), _minInliers, signatureA->id(), signatureB->id());
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
(int)signatureA->getWords3().size(), (int)signatureB->getWords3().size(), _minInliers);
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("No camera transform found");
UINFO(msg.c_str());
}
}
else if(words3From->size() == 0)
{
msg = uFormat("No 3D guess words found");
UWARN(msg.c_str());
}
else
{
msg = uFormat("No camera model");
UWARN(msg.c_str());
}
}
else if(_estimationType == 1) // PnP
{
if(!toSignature.sensorData().stereoCameraModel().isValid() &&
(toSignature.sensorData().cameraModels().size() != 1 ||
!toSignature.sensorData().cameraModels()[0].isValid()))
{
UERROR("Calibrated camera required (multi-cameras not supported). Id=%d Models=%d StereoModel=%d weight=%d",
toSignature.id(),
(int)toSignature.sensorData().cameraModels().size(),
toSignature.sensorData().stereoCameraModel().isValid()?1:0,
toSignature.getWeight());
}
else
{
// 3D to 2D
if((int)words3From->size() >= _minInliers &&
(int)wordsTo->size() >= _minInliers)
{
UASSERT(toSignature.sensorData().stereoCameraModel().isValid() || (toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModel().isValid()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0];
std::vector<int> inliersV;
transform = util3d::estimateMotion3DTo2D(
uMultimapToMap(*words3From),
uMultimapToMap(*wordsTo),
cameraModel,
_minInliers,
_iterations,
_PnPReprojError,
_PnPFlags,
Transform::getIdentity(),
uMultimapToMap(*words3To),
_varianceFromInliersCount?0:&variance,
0,
&inliersV);
inliersCount = (int)inliersV.size();
if(transform.isNull())
if(!transforms[1].isNull())
{
transforms[1] = transforms[1].inverse();
}
UDEBUG("t1=%s", transforms[0].prettyPrint().c_str());
UDEBUG("t2=%s", transforms[1].prettyPrint().c_str());
if(!transforms[1].isNull())
{
if(transforms[0].isNull())
{
transform = transforms[1];
if(inliersOut)
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
inliersCount, _minInliers, fromSignature.id(), toSignature.id());
UINFO(msg.c_str());
*inliersOut = inliers[1];
}
variance = variances[1];
if(_varianceFromInliersCount)
{
variance = inliers[1].size() > 0?1.0f/float(inliers[1].size()):1.0f;
}
}
else
{
msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
(int)words3From->size(), (int)wordsTo->size(), _minInliers);
UINFO(msg.c_str());
}
}
}
else
{
// 3D -> 3D
if((int)words3From->size() >= _minInliers &&
(int)words3To->size() >= _minInliers)
{
std::vector<int> inliersV;
transform = util3d::estimateMotion3DTo3D(
uMultimapToMap(*words3From),
uMultimapToMap(*words3To),
_minInliers,
_inlierDistance,
_iterations,
_refineIterations,
&variance,
0,
&inliersV);
inliersCount = (int)inliersV.size();
if(transform.isNull())
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
inliersCount, _minInliers, fromSignature.id(), toSignature.id());
UINFO(msg.c_str());
transform = transforms[0].interpolate(0.5f, transforms[1]);
if(inliersOut)
{
*inliersOut = inliers[0];
}
variance = (variances[0]+variances[1])/2.0f;
if(_varianceFromInliersCount)
{
int avg = (inliers[0].size()+inliers[1].size())/2;
variance = avg>0?1.0f/float(avg):1.0f;
}
}
}
else
{
msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
(int)words3From->size(), (int)words3To->size(), _minInliers);
UINFO(msg.c_str());
transform = transforms[0];
if(inliersOut)
{
*inliersOut = inliers[0];
}
variance = variances[0];
if(_varianceFromInliersCount)
{
variance = inliers[0].size() > 0?1.0f/float(inliers[0].size()):1.0f;
}
}
}
if(!transform.isNull())
{
UDEBUG("");
// verify if it is a 180 degree transform, well verify > 90
float x,y,z, roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
@@ -395,19 +769,10 @@ Transform RegistrationVis::computeTransformation(
}
}
if(_varianceFromInliersCount)
{
variance = inliersCount > 0?1.0/double(inliersCount):1.0;
}
if(rejectedMsg)
{
*rejectedMsg = msg;
}
if(inliersOut)
{
*inliersOut = inliersCount;
}
if(varianceOut)
{
*varianceOut = variance>0.0f?variance:0.0001; // epsilon if exact transform

2
corelib/src/Rtabmap.cpp
View File

@@ -3114,7 +3114,7 @@ void Rtabmap::get3DMap(
SensorData data = _memory->getNodeData(*iter);
data.setId(*iter);
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, pcl::PointXYZ> words3;
std::multimap<int, cv::Point3f> words3;
_memory->getNodeWords(*iter, words, words3);
signatures.insert(std::make_pair(*iter,
Signature(*iter,

22
corelib/src/Signature.cpp
View File

@@ -75,6 +75,22 @@ Signature::Signature(
UASSERT(_sensorData.id() == _id);
}
Signature::Signature(const SensorData & data) :
_id(data.id()),
_mapId(-1),
_stamp(data.stamp()),
_weight(0),
_label(""),
_saved(false),
_modified(true),
_linksModified(true),
_enabled(false),
_pose(Transform::getIdentity()),
_sensorData(data)
{
}
Signature::~Signature()
{
//UDEBUG("id=%d", _id);
@@ -175,7 +191,7 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
std::list<cv::KeyPoint> kps = uValues(_words, oldWordId);
if(kps.size())
{
std::list<pcl::PointXYZ> pts = uValues(_words3, oldWordId);
std::list<cv::Point3f> pts = uValues(_words3, oldWordId);
_words.erase(oldWordId);
_words3.erase(oldWordId);
_wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
@@ -183,9 +199,9 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
{
_words.insert(std::pair<int, cv::KeyPoint>(activeWordId, (*iter)));
}
for(std::list<pcl::PointXYZ>::const_iterator iter=pts.begin(); iter!=pts.end(); ++iter)
for(std::list<cv::Point3f>::const_iterator iter=pts.begin(); iter!=pts.end(); ++iter)
{
_words3.insert(std::pair<int, pcl::PointXYZ>(activeWordId, (*iter)));
_words3.insert(std::pair<int, cv::Point3f>(activeWordId, (*iter)));
}
}
}

14
corelib/src/Stereo.cpp
View File

@@ -32,6 +32,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
Stereo * Stereo::create(const ParametersMap & parameters)
{
bool opticalFlow = Parameters::defaultStereoOpticalFlow();
Parameters::parse(parameters, Parameters::kStereoOpticalFlow(), opticalFlow);
if(opticalFlow)
{
return new StereoOpticalFlow(parameters);
}
else
{
return new Stereo(parameters);
}
}
Stereo::Stereo(const ParametersMap & parameters) :
winWidth_(Parameters::defaultStereoWinWidth()),
winHeight_(Parameters::defaultStereoWinHeight()),

14
corelib/src/Transform.cpp
View File

@@ -67,21 +67,22 @@ Transform::Transform(float x, float y, float z, float roll, float pitch, float y
*this = fromEigen3f(t);
}
Transform::Transform(float x, float y, float z, float qx, float qy, float qz, float qw)
Transform::Transform(float x, float y, float z, float qx, float qy, float qz, float qw) :
data_(cv::Mat::zeros(3,4,CV_32FC1))
{
Eigen::Matrix3f rotation = Eigen::Quaternionf(qw, qx, qy, qz).toRotationMatrix();
data()[0] = rotation(0,0);
data()[1] = rotation(0,1);
data()[2] = rotation(0,2);
data()[3] = 0.0f;
data()[3] = x;
data()[4] = rotation(1,0);
data()[5] = rotation(1,1);
data()[6] = rotation(1,2);
data()[7] = 0.0f;
data()[7] = y;
data()[8] = rotation(2,0);
data()[9] = rotation(2,1);
data()[10] = rotation(2,2);
data()[11] = 0.0f;
data()[11] = z;
}
Transform::Transform(float x, float y, float theta)
@@ -92,7 +93,8 @@ Transform::Transform(float x, float y, float theta)
bool Transform::isNull() const
{
return (data()[0] == 0.0f &&
return (data_.empty() ||
(data()[0] == 0.0f &&
data()[1] == 0.0f &&
data()[2] == 0.0f &&
data()[3] == 0.0f &&
@@ -115,7 +117,7 @@ bool Transform::isNull() const
uIsNan(data()[8]) ||
uIsNan(data()[9]) ||
uIsNan(data()[10]) ||
uIsNan(data()[11]);
uIsNan(data()[11]));
}
bool Transform::isIdentity() const

17
corelib/src/VWDictionary.cpp
View File

@@ -686,16 +686,19 @@ void VWDictionary::update()
UDEBUG("");
}
void VWDictionary::clear()
void VWDictionary::clear(bool printWarningsIfNotEmpty)
{
ULOGGER_DEBUG("");
if(_visualWords.size() && _incrementalDictionary)
if(printWarningsIfNotEmpty)
{
UWARN("Visual dictionary would be already empty here (%d words still in dictionary).", (int)_visualWords.size());
}
if(_notIndexedWords.size())
{
UWARN("Not indexed words should be empty here (%d words still not indexed)", (int)_notIndexedWords.size());
if(_visualWords.size() && _incrementalDictionary)
{
UWARN("Visual dictionary would be already empty here (%d words still in dictionary).", (int)_visualWords.size());
}
if(_notIndexedWords.size())
{
UWARN("Not indexed words should be empty here (%d words still not indexed)", (int)_notIndexedWords.size());
}
}
for(std::map<int, VisualWord *>::iterator i=_visualWords.begin(); i!=_visualWords.end(); ++i)
{

42
corelib/src/util3d.cpp
View File

@@ -381,7 +381,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
{
float disp = float(imageDisparity.at<short>(h,w))/16.0f;
cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
}
}
}
@@ -392,7 +393,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
{
float disp = imageDisparity.at<float>(h,w);
cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
cv::Point3f pt = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
cloud->at((h/decimation)*cloud->width + (w/decimation)) = pcl::PointXYZ(pt.x, pt.y, pt.z);
}
}
}
@@ -451,7 +453,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
}
float disp = imageDisparity.type()==CV_16SC1?float(imageDisparity.at<short>(h,w))/16.0f:imageDisparity.at<float>(h,w);
pcl::PointXYZ ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
cv::Point3f ptXYZ = projectDisparityTo3D(cv::Point2f(w, h), disp, model);
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
@@ -858,7 +860,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
}
// inspired from ROS image_geometry/src/stereo_camera_model.cpp
pcl::PointXYZ projectDisparityTo3D(
cv::Point3f projectDisparityTo3D(
const cv::Point2f & pt,
float disparity,
const StereoCameraModel & model)
@@ -867,13 +869,13 @@ pcl::PointXYZ projectDisparityTo3D(
{
//Z = baseline * f / (d + cx1-cx0);
float W = model.baseline()/(disparity + model.right().cx() - model.left().cx());
return pcl::PointXYZ((pt.x - model.left().cx())*W, (pt.y - model.left().cy())*W, model.left().fx()*W);
return cv::Point3f((pt.x - model.left().cx())*W, (pt.y - model.left().cy())*W, model.left().fx()*W);
}
float bad_point = std::numeric_limits<float>::quiet_NaN ();
return pcl::PointXYZ(bad_point, bad_point, bad_point);
return cv::Point3f(bad_point, bad_point, bad_point);
}
pcl::PointXYZ projectDisparityTo3D(
cv::Point3f projectDisparityTo3D(
const cv::Point2f & pt,
const cv::Mat & disparity,
const StereoCameraModel & model)
@@ -888,7 +890,12 @@ pcl::PointXYZ projectDisparityTo3D(
float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
return projectDisparityTo3D(pt, d, model);
}
return pcl::PointXYZ(bad_point, bad_point, bad_point);
return cv::Point3f(bad_point, bad_point, bad_point);
}
bool isFinite(const cv::Point3f & pt)
{
return uIsFinite(pt.x) && uIsFinite(pt.y) && uIsFinite(pt.z);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr concatenateClouds(const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds)
@@ -961,6 +968,25 @@ void savePCDWords(
}
}
void savePCDWords(
const std::string & fileName,
const std::multimap<int, cv::Point3f> & words,
const Transform & transform)
{
if(words.size())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.resize(words.size());
int i=0;
for(std::multimap<int, cv::Point3f>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
cv::Point3f pt = transformPoint(iter->second, transform);
cloud[i++] = pcl::PointXYZ(pt.x, pt.y, pt.z);
}
pcl::io::savePCDFile(fileName, cloud);
}
}
pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, int dim)
{
UASSERT(dim > 0);

24
corelib/src/util3d_correspondences.cpp
View File

@@ -315,10 +315,10 @@ void findCorrespondences(
}
void findCorrespondences(
const std::multimap<int, pcl::PointXYZ> & words1,
const std::multimap<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
const std::multimap<int, cv::Point3f> & words1,
const std::multimap<int, cv::Point3f> & words2,
std::vector<cv::Point3f> & inliers1,
std::vector<cv::Point3f> & inliers2,
float maxDepth,
std::vector<int> * uniqueCorrespondences)
{
@@ -338,8 +338,8 @@ void findCorrespondences(
{
inliers1[oi] = words1.find(*iter)->second;
inliers2[oi] = words2.find(*iter)->second;
if(pcl::isFinite(inliers1[oi]) &&
pcl::isFinite(inliers2[oi]) &&
if(util3d::isFinite(inliers1[oi]) &&
util3d::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 > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
@@ -361,10 +361,10 @@ void findCorrespondences(
}
void findCorrespondences(
const std::map<int, pcl::PointXYZ> & words1,
const std::map<int, pcl::PointXYZ> & words2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
const std::map<int, cv::Point3f> & words1,
const std::map<int, cv::Point3f> & words2,
std::vector<cv::Point3f> & inliers1,
std::vector<cv::Point3f> & inliers2,
float maxDepth,
std::vector<int> * correspondences)
{
@@ -384,8 +384,8 @@ void findCorrespondences(
{
inliers1[oi] = words1.find(*iter)->second;
inliers2[oi] = words2.find(*iter)->second;
if(pcl::isFinite(inliers1[oi]) &&
pcl::isFinite(inliers2[oi]) &&
if(util3d::isFinite(inliers1[oi]) &&
util3d::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 > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))

103
corelib/src/util3d_features.cpp
View File

@@ -31,12 +31,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UStl.h>
#include <opencv2/video/tracking.hpp>
@@ -46,7 +48,7 @@ namespace rtabmap
namespace util3d
{
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
std::vector<cv::Point3f> generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const CameraModel & cameraModel)
@@ -57,26 +59,26 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
return generateKeypoints3DDepth(keypoints, depth, models);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
std::vector<cv::Point3f> generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const std::vector<CameraModel> & cameraModels)
{
UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
UASSERT(cameraModels.size());
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<cv::Point3f> keypoints3d;
if(!depth.empty())
{
UASSERT(int((depth.cols/cameraModels.size())*cameraModels.size()) == depth.cols);
float subImageWidth = depth.cols/cameraModels.size();
keypoints3d->resize(keypoints.size());
keypoints3d.resize(keypoints.size());
for(unsigned int i=0; i!=keypoints.size(); ++i)
{
int cameraIndex = int(keypoints[i].pt.x / subImageWidth);
UASSERT_MSG(cameraIndex < (int)cameraModels.size(),
uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f",
cameraIndex, (int)cameraModels.size(), keypoints[i].pt.x, subImageWidth).c_str());
pcl::PointXYZ pt = util3d::projectDepthTo3D(
pcl::PointXYZ ptXYZ = util3d::projectDepthTo3D(
depth,
keypoints[i].pt.x-subImageWidth*cameraIndex,
keypoints[i].pt.y,
@@ -86,46 +88,47 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
cameraModels.at(cameraIndex).fy(),
true);
if(pcl::isFinite(pt) &&
cv::Point3f pt(ptXYZ.x, ptXYZ.y, ptXYZ.z);
if(util3d::isFinite(pt) &&
!cameraModels.at(cameraIndex).localTransform().isNull() &&
!cameraModels.at(cameraIndex).localTransform().isIdentity())
{
pt = util3d::transformPoint(pt, cameraModels.at(cameraIndex).localTransform());
}
keypoints3d->at(i) = pt;
keypoints3d.at(i) = pt;
}
}
return keypoints3d;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
std::vector<cv::Point3f> generateKeypoints3DDisparity(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
const StereoCameraModel & stereoCameraModel)
{
UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F));
UASSERT(stereoCameraModel.isValid());
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
keypoints3d->resize(keypoints.size());
std::vector<cv::Point3f> keypoints3d;
keypoints3d.resize(keypoints.size());
for(unsigned int i=0; i!=keypoints.size(); ++i)
{
pcl::PointXYZ pt = util3d::projectDisparityTo3D(
cv::Point3f pt = util3d::projectDisparityTo3D(
keypoints[i].pt,
disparity,
stereoCameraModel);
if(pcl::isFinite(pt) &&
if(util3d::isFinite(pt) &&
!stereoCameraModel.left().localTransform().isNull() &&
!stereoCameraModel.left().localTransform().isIdentity())
{
pt = util3d::transformPoint(pt, stereoCameraModel.left().localTransform());
}
keypoints3d->at(i) = pt;
keypoints3d.at(i) = pt;
}
return keypoints3d;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
std::vector<cv::Point3f> generateKeypoints3DStereo(
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
const StereoCameraModel & model,
@@ -135,23 +138,23 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
UASSERT(mask.size() == 0 || leftCorners.size() == mask.size());
UASSERT(model.left().fx()> 0.0f && model.baseline() > 0.0f);
pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
keypoints3d->resize(leftCorners.size());
std::vector<cv::Point3f> keypoints3d;
keypoints3d.resize(leftCorners.size());
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(unsigned int i=0; i<leftCorners.size(); ++i)
{
pcl::PointXYZ pt(bad_point, bad_point, bad_point);
cv::Point3f pt(bad_point, bad_point, bad_point);
if(mask.empty() || mask[i])
{
float disparity = leftCorners[i].x - rightCorners[i].x;
if(disparity != 0.0f)
{
pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
cv::Point3f tmpPt = util3d::projectDisparityTo3D(
leftCorners[i],
disparity,
model);
if(pcl::isFinite(tmpPt))
if(util3d::isFinite(tmpPt))
{
pt = tmpPt;
if(!model.localTransform().isNull() &&
@@ -163,7 +166,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
}
}
keypoints3d->at(i) = pt;
keypoints3d.at(i) = pt;
}
return keypoints3d;
}
@@ -172,23 +175,24 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
// 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,
std::map<int, cv::Point3f> generateWords3DMono(
const std::map<int, cv::KeyPoint> & refWords,
const std::map<int, cv::KeyPoint> & nextWords,
const CameraModel & cameraModel,
Transform & cameraTransform,
int pnpIterations,
float pnpReprojError,
int pnpFlags,
bool pnpOpenCV2,
float ransacParam1,
float ransacParam2,
const std::multimap<int, pcl::PointXYZ> & refGuess3D,
const std::map<int, cv::Point3f> & refGuess3D,
double * varianceOut)
{
UASSERT(cameraModel.isValid());
std::multimap<int, pcl::PointXYZ> words3D;
std::map<int, cv::Point3f> words3D;
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
if(EpipolarGeometry::findPairsUnique(refWords, nextWords, pairs) > 8)
if(EpipolarGeometry::findPairs(refWords, nextWords, pairs) > 8)
{
std::vector<unsigned char> status;
cv::Mat F = EpipolarGeometry::findFFromWords(pairs, status, ransacParam1, ransacParam2);
@@ -268,7 +272,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
// triangulate the points
//std::vector<double> reprojErrors;
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
//std::vector<cv::Point3f> cloud;
//EpipolarGeometry::triangulatePoints(x_norm, xp_norm, P0, P, cloud, reprojErrors);
cv::Mat pts4D;
cv::triangulatePoints(P0, P, x_norm, xp_norm, pts4D);
@@ -282,23 +286,23 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
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)), cameraModel.localTransform())));
words3D.insert(std::make_pair(indexes[i], util3d::transformPoint(cv::Point3f(pts4D.at<double>(0,i), pts4D.at<double>(1,i), pts4D.at<double>(2,i)), cameraModel.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>);
std::vector<cv::Point3f> inliersRef;
std::vector<cv::Point3f> inliersRefGuess;
util3d::findCorrespondences(
words3D,
refGuess3D,
*inliersRef,
*inliersRefGuess,
inliersRef,
inliersRefGuess,
0);
if(inliersRef->size())
if(inliersRef.size())
{
// estimate the scale
float scale = 1.0f;
@@ -306,18 +310,18 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
if(!useCameraTransformGuess)
{
std::multimap<float, float> scales; // <variance, scale>
for(unsigned int i=0; i<inliersRef->size(); ++i)
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)
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);
cv::Point3f refPt = inliersRef.at(j);
refPt.x *= s;
refPt.y *= s;
refPt.z *= s;
const pcl::PointXYZ & newPt = inliersRefGuess->at(j);
const cv::Point3f & 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());
@@ -333,11 +337,11 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
else
{
//compute variance at scale=1
std::vector<float> errorSqrdDists(inliersRef->size());
for(unsigned int j=0; j<inliersRef->size(); ++j)
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);
const cv::Point3f & refPt = inliersRef.at(j);
const cv::Point3f & 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());
@@ -358,8 +362,8 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
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))
std::multimap<int, cv::Point3f>::iterator iter = words3D.find(indexes[i]);
if(util3d::isFinite(iter->second))
{
iter->second.x *= scale;
iter->second.y *= scale;
@@ -384,7 +388,7 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
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(
util3d::solvePnPRansac(
objectPoints,
imagePoints,
K,
@@ -394,13 +398,10 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
true,
pnpIterations,
pnpReprojError,
#if CV_MAJOR_VERSION < 3
0, // min inliers
#else
0.99, // confidence
#endif
inliersV,
pnpFlags);
pnpFlags,
pnpOpenCV2);
UDEBUG("PnP inliers = %d / %d", (int)inliersV.size(), (int)objectPoints.size());

266
corelib/src/util3d_motion_estimation.cpp
View File

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/util3d.h"
namespace rtabmap
{
@@ -40,29 +41,23 @@ namespace rtabmap
namespace util3d
{
#if CV_MAJOR_VERSION >= 3
void solvePnPRansac(cv::InputArray _opoints, cv::InputArray _ipoints,
cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
cv::OutputArray _rvec, cv::OutputArray _tvec, bool useExtrinsicGuess,
int iterationsCount, float reprojectionError, int minInliersCount,
cv::OutputArray _inliers, int flags);
#endif
Transform estimateMotion3DTo2D(
const std::map<int, pcl::PointXYZ> & words3A,
const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::KeyPoint> & words2B,
const CameraModel & cameraModel,
int minInliers,
int iterations,
double reprojError,
int flagsPnP,
bool pnpOpenCV2,
const Transform & guess,
const std::map<int, pcl::PointXYZ> & words3B,
const std::map<int, cv::Point3f> & words3B,
double * varianceOut,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut)
{
UASSERT(cameraModel.isValid());
UASSERT(!guess.isNull());
Transform transform;
std::vector<int> matches, inliers;
@@ -79,9 +74,10 @@ Transform estimateMotion3DTo2D(
matches.resize(ids.size());
for(unsigned int i=0; i<ids.size(); ++i)
{
if(words3A.find(ids[i]) != words3A.end())
std::map<int, cv::Point3f>::const_iterator iter=words3A.find(ids[i]);
if(iter != words3A.end() && util3d::isFinite(iter->second))
{
pcl::PointXYZ pt = words3A.find(ids[i])->second;
cv::Point3f pt = words3A.find(ids[i])->second;
objectPoints[oi].x = pt.x;
objectPoints[oi].y = pt.y;
objectPoints[oi].z = pt.z;
@@ -109,11 +105,7 @@ Transform estimateMotion3DTo2D(
cv::Mat tvec = (cv::Mat_<double>(1,3) <<
(double)guessCameraFrame.x(), (double)guessCameraFrame.y(), (double)guessCameraFrame.z());
#if CV_MAJOR_VERSION >= 3
solvePnPRansac(
#else
cv::solvePnPRansac(
#endif
util3d::solvePnPRansac(
objectPoints,
imagePoints,
K,
@@ -125,7 +117,8 @@ Transform estimateMotion3DTo2D(
reprojError,
0, // min inliers
inliers,
flagsPnP);
flagsPnP,
pnpOpenCV2);
if((int)inliers.size() >= minInliers)
{
@@ -143,11 +136,11 @@ Transform estimateMotion3DTo2D(
oi = 0;
for(unsigned int i=0; i<inliers.size(); ++i)
{
std::map<int, pcl::PointXYZ>::const_iterator iter = words3B.find(matches[inliers[i]]);
if(iter != words3B.end() && pcl::isFinite(iter->second))
std::map<int, cv::Point3f>::const_iterator iter = words3B.find(matches[inliers[i]]);
if(iter != words3B.end() && util3d::isFinite(iter->second))
{
const cv::Point3f & objPt = objectPoints[inliers[i]];
pcl::PointXYZ newPt = util3d::transformPoint(iter->second, transform);
cv::Point3f newPt = util3d::transformPoint(iter->second, transform);
errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
}
}
@@ -191,8 +184,8 @@ Transform estimateMotion3DTo2D(
}
Transform estimateMotion3DTo3D(
const std::map<int, pcl::PointXYZ> & words3A,
const std::map<int, pcl::PointXYZ> & words3B,
const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::Point3f> & words3B,
int minInliers,
double inliersDistance,
int iterations,
@@ -202,29 +195,43 @@ Transform estimateMotion3DTo3D(
std::vector<int> * inliersOut)
{
Transform transform;
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
std::vector<cv::Point3f> inliers1; // previous
std::vector<cv::Point3f> inliers2; // new
std::vector<int> matches;
util3d::findCorrespondences(
words3A,
words3B,
*inliers1,
*inliers2,
inliers1,
inliers2,
0,
&matches);
UASSERT(inliers1.size() == inliers2.size());
if(varianceOut)
{
*varianceOut = 1.0;
}
if((int)inliers1->size() >= minInliers)
if((int)inliers1.size() >= minInliers)
{
std::vector<int> inliers;
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2cloud(new pcl::PointCloud<pcl::PointXYZ>);
inliers1cloud->resize(inliers1.size());
inliers2cloud->resize(inliers1.size());
for(unsigned int i=0; i<inliers1.size(); ++i)
{
(*inliers1cloud)[i].x = inliers1[i].x;
(*inliers1cloud)[i].y = inliers1[i].y;
(*inliers1cloud)[i].z = inliers1[i].z;
(*inliers2cloud)[i].x = inliers2[i].x;
(*inliers2cloud)[i].y = inliers2[i].y;
(*inliers2cloud)[i].z = inliers2[i].z;
}
Transform t = util3d::transformFromXYZCorrespondences(
inliers2,
inliers1,
inliers2cloud,
inliers1cloud,
inliersDistance,
iterations,
refineIterations>0,
@@ -463,89 +470,124 @@ namespace pnpransac
cv::Mutex PnPSolver::syncMutex;
}
void solvePnPRansac(cv::InputArray _opoints, cv::InputArray _ipoints,
cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
cv::OutputArray _rvec, cv::OutputArray _tvec, bool useExtrinsicGuess,
int iterationsCount, float reprojectionError, int minInliersCount,
cv::OutputArray _inliers, int flags)
{
const int _rng_seed = 0;
cv::Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
cv::Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
CV_Assert(opoints.isContinuous());
CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F);
CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
CV_Assert(ipoints.isContinuous());
CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
_rvec.create(3, 1, CV_64FC1);
_tvec.create(3, 1, CV_64FC1);
cv::Mat rvec = _rvec.getMat();
cv::Mat tvec = _tvec.getMat();
cv::Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);
if (minInliersCount <= 0)
minInliersCount = objectPoints.cols;
pnpransac::Parameters params;
params.iterationsCount = iterationsCount;
params.minInliersCount = minInliersCount;
params.reprojectionError = reprojectionError;
params.useExtrinsicGuess = useExtrinsicGuess;
params.camera.init(cameraMatrix, distCoeffs);
params.flags = flags;
std::vector<int> localInliers;
cv::Mat localRvec, localTvec;
rvec.copyTo(localRvec);
tvec.copyTo(localTvec);
int bestIndex;
// TBB not used
if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT)
{
pnpransac::PnPSolver solver(objectPoints, imagePoints, params,
localRvec, localTvec, localInliers, bestIndex,
_rng_seed);
solver(0, iterationsCount);
}
if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT)
{
if (flags != CV_P3P)
{
int i, pointsCount = (int)localInliers.size();
cv::Mat inlierObjectPoints(1, pointsCount, CV_MAKE_TYPE(opoints.depth(), 3)), inlierImagePoints(1, pointsCount, CV_MAKE_TYPE(ipoints.depth(), 2));
for (i = 0; i < pointsCount; i++)
{
int index = localInliers[i];
cv::Mat colInlierImagePoints = inlierImagePoints(cv::Rect(i, 0, 1, 1));
imagePoints.col(index).copyTo(colInlierImagePoints);
cv::Mat colInlierObjectPoints = inlierObjectPoints(cv::Rect(i, 0, 1, 1));
objectPoints.col(index).copyTo(colInlierObjectPoints);
}
solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, false, flags);
}
localRvec.copyTo(rvec);
localTvec.copyTo(tvec);
if (_inliers.needed())
cv::Mat(localInliers).copyTo(_inliers);
}
else
{
tvec.setTo(cv::Scalar(0));
cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
Rodrigues(R, rvec);
if( _inliers.needed() )
_inliers.release();
}
return;
}
#endif
void solvePnPRansac(
cv::InputArray _opoints,
cv::InputArray _ipoints,
cv::InputArray _cameraMatrix,
cv::InputArray _distCoeffs,
cv::OutputArray _rvec,
cv::OutputArray _tvec,
bool useExtrinsicGuess,
int iterationsCount,
float reprojectionError,
int minInliersCount,
cv::OutputArray _inliers,
int flags,
bool opencv2version)
{
#if CV_MAJOR_VERSION >= 3
if(opencv2version)
{
const int _rng_seed = 0;
cv::Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
cv::Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
CV_Assert(opoints.isContinuous());
CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F);
CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
CV_Assert(ipoints.isContinuous());
CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
_rvec.create(3, 1, CV_64FC1);
_tvec.create(3, 1, CV_64FC1);
cv::Mat rvec = _rvec.getMat();
cv::Mat tvec = _tvec.getMat();
cv::Mat objectPoints = opoints.reshape(3, 1), imagePoints = ipoints.reshape(2, 1);
if (minInliersCount <= 0)
minInliersCount = objectPoints.cols;
pnpransac::Parameters params;
params.iterationsCount = iterationsCount;
params.minInliersCount = minInliersCount;
params.reprojectionError = reprojectionError;
params.useExtrinsicGuess = useExtrinsicGuess;
params.camera.init(cameraMatrix, distCoeffs);
params.flags = flags;
std::vector<int> localInliers;
cv::Mat localRvec, localTvec;
rvec.copyTo(localRvec);
tvec.copyTo(localTvec);
int bestIndex;
// TBB not used
if (objectPoints.cols >= pnpransac::MIN_POINTS_COUNT)
{
pnpransac::PnPSolver solver(objectPoints, imagePoints, params,
localRvec, localTvec, localInliers, bestIndex,
_rng_seed);
solver(0, iterationsCount);
}
if (localInliers.size() >= (size_t)pnpransac::MIN_POINTS_COUNT)
{
if (flags != CV_P3P)
{
int i, pointsCount = (int)localInliers.size();
cv::Mat inlierObjectPoints(1, pointsCount, CV_MAKE_TYPE(opoints.depth(), 3)), inlierImagePoints(1, pointsCount, CV_MAKE_TYPE(ipoints.depth(), 2));
for (i = 0; i < pointsCount; i++)
{
int index = localInliers[i];
cv::Mat colInlierImagePoints = inlierImagePoints(cv::Rect(i, 0, 1, 1));
imagePoints.col(index).copyTo(colInlierImagePoints);
cv::Mat colInlierObjectPoints = inlierObjectPoints(cv::Rect(i, 0, 1, 1));
objectPoints.col(index).copyTo(colInlierObjectPoints);
}
solvePnP(inlierObjectPoints, inlierImagePoints, params.camera.intrinsics, params.camera.distortion, localRvec, localTvec, false, flags);
}
localRvec.copyTo(rvec);
localTvec.copyTo(tvec);
if (_inliers.needed())
cv::Mat(localInliers).copyTo(_inliers);
}
else
{
tvec.setTo(cv::Scalar(0));
cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
Rodrigues(R, rvec);
if( _inliers.needed() )
_inliers.release();
}
}
else
#endif
{
cv::solvePnPRansac(
_opoints,
_ipoints,
_cameraMatrix,
_distCoeffs,
_rvec,
_tvec,
useExtrinsicGuess,
iterationsCount,
reprojectionError,
#if CV_MAJOR_VERSION < 3
minInliersCount, // min inliers
#else
0.99, // confidence
#endif
_inliers,
flags);
}
return;
}
}
}

10
corelib/src/util3d_transforms.cpp
View File

@@ -68,6 +68,16 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformPointCloud(
return output;
}
cv::Point3f transformPoint(
const cv::Point3f & point,
const Transform & transform)
{
cv::Point3f ret = point;
ret.x = transform (0, 0) * point.x + transform (0, 1) * point.y + transform (0, 2) * point.z + transform (0, 3);
ret.y = transform (1, 0) * point.x + transform (1, 1) * point.y + transform (1, 2) * point.z + transform (1, 3);
ret.z = transform (2, 0) * point.x + transform (2, 1) * point.y + transform (2, 2) * point.z + transform (2, 3);
return ret;
}
pcl::PointXYZ transformPoint(
const pcl::PointXYZ & pt,
const Transform & transform)