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

Added Monocular SLAM (experimental), Odometry classes refactoring

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

3
corelib/include/rtabmap/core/CameraRGBD.h
View File

@@ -96,11 +96,13 @@ public:
float getImageRate() const {return _imageRate;}
const Transform & getLocalTransform() const {return _localTransform;}
bool isMirroringEnabled() const {return _mirroring;}
bool isColorOnly() const {return _colorOnly;}
//setters
void setImageRate(float imageRate) {_imageRate = imageRate;}
void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
void setMirroringEnabled(bool mirroring) {_mirroring = mirroring;}
void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
protected:
/**
@@ -120,6 +122,7 @@ private:
float _imageRate;
Transform _localTransform;
bool _mirroring;
bool _colorOnly;
UTimer * _frameRateTimer;
};

11
corelib/include/rtabmap/core/Features2d.h
View File

@@ -89,21 +89,26 @@ public:
static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
int getMaxFeatures() const {return maxFeatures_;}
public:
virtual ~Feature2D() {}
std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image, int maxKeypoints=0, const cv::Rect & roi = cv::Rect()) const;
std::vector<cv::KeyPoint> generateKeypoints(const cv::Mat & image, const cv::Rect & roi = cv::Rect()) const;
cv::Mat generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
virtual void parseParameters(const ParametersMap & parameters) {}
virtual void parseParameters(const ParametersMap & parameters);
virtual Feature2D::Type getType() const = 0;
protected:
Feature2D(const ParametersMap & parameters = ParametersMap()) {}
Feature2D(const ParametersMap & parameters = ParametersMap());
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi) const = 0;
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
private:
int maxFeatures_;
};
//SURF

3
corelib/include/rtabmap/core/Memory.h
View File

@@ -245,7 +245,6 @@ private:
bool _tfIdfLikelihoodUsed;
bool _parallelized;
float _wordsMaxDepth; // 0=inf
int _wordsPerImageTarget; // <0=none, 0=inf
std::vector<float> _roiRatios; // size 4
// RGBD-SLAM stuff
@@ -254,6 +253,8 @@ private:
int _bowIterations;
float _bowMaxDepth;
bool _bowForce2D;
bool _bowEpipolarGeometry;
float _bowEpipolarGeometryVar;
int _icpDecimation;
float _icpMaxDepth;
float _icpVoxelSize;

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

@@ -30,20 +30,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/RtabmapExp.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UEventsHandler.h>
#include <rtabmap/utilite/UEvent.h>
#include <rtabmap/utilite/UMutex.h>
#include <rtabmap/utilite/USemaphore.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/SensorData.h>
#include <rtabmap/core/OdometryInfo.h>
#include <opencv2/opencv.hpp>
#include <pcl/common/eigen.h>
#include <rtabmap/core/Parameters.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
@@ -52,6 +41,7 @@ class UTimer;
namespace rtabmap {
class Feature2D;
class OdometryInfo;
class RTABMAP_EXP Odometry
{
@@ -62,7 +52,6 @@ public:
//getters
const Transform & getPose() const {return _pose;}
int getMaxFeatures() const {return _maxFeatures;}
const std::string & getRoiRatios() const {return _roiRatios;}
int getMinInliers() const {return _minInliers;}
float getInlierDistance() const {return _inlierDistance;}
@@ -71,14 +60,13 @@ public:
float getMaxDepth() const {return _maxDepth;}
bool isInfoDataFilled() const {return _fillInfoData;}
bool isPnPEstimationUsed() const {return _pnpEstimation;}
double getPnPReprojError() const {return _pnpReprojError;}
double getPnPReprojError() const {return _pnpReprojError;}
int getPnPFlags() const {return _pnpFlags;}
private:
virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
private:
int _maxFeatures;
std::string _roiRatios;
int _minInliers;
float _inlierDistance;
@@ -163,6 +151,38 @@ private:
pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_;
};
class OdometryMono : public Odometry
{
public:
OdometryMono(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
virtual ~OdometryMono();
virtual void reset(const Transform & initialPose);
private:
virtual Transform computeTransform(const SensorData & data, OdometryInfo * info = 0);
private:
//Parameters:
int flowWinSize_;
int flowIterations_;
double flowEps_;
int flowMaxLevel_;
Memory * memory_;
int localHistoryMaxSize_;
float initMinFlow_;
float initMinTranslation_;
float minTranslation_;
float fundMatrixReprojError_;
float fundMatrixConfidence_;
cv::Mat refDepth_;
std::map<int, cv::Point2f> cornersMap_;
std::multimap<int, cv::Point3f> localMap_;
std::map<int, std::multimap<int, pcl::PointXYZ> > keyFrameWords3D_;
std::map<int, Transform> keyFramePoses_;
float maxVariance_;
};
class RTABMAP_EXP OdometryICP : public Odometry
{
public:
@@ -192,32 +212,5 @@ private:
pcl::PointCloud<pcl::PointXYZ>::Ptr _previousCloud; // for point to point
};
class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler {
public:
// take ownership of Odometry
OdometryThread(Odometry * odometry);
virtual ~OdometryThread();
protected:
virtual void handleEvent(UEvent * event);
private:
void mainLoopKill();
//============================================================
// MAIN LOOP
//============================================================
void mainLoop();
void addData(const SensorData & data);
void getData(SensorData & data);
private:
USemaphore _dataAdded;
UMutex _dataMutex;
SensorData _dataBuffer;
Odometry * _odometry;
bool _resetOdometry;
};
} /* namespace rtabmap */
#endif /* ODOMETRY_H_ */

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

@@ -57,10 +57,11 @@ public:
std::multimap<int, cv::KeyPoint> words;
std::vector<int> wordMatches;
std::vector<int> wordInliers;
std::multimap<int, cv::Point3f> localMap;
// Optical Flow odometry
std::vector<cv::KeyPoint> refCorners;
std::vector<cv::KeyPoint> newCorners;
std::vector<cv::Point2f> refCorners;
std::vector<cv::Point2f> newCorners;
std::vector<int> cornerInliers;
};

70
corelib/include/rtabmap/core/OdometryThread.h Normal file
View File

@@ -0,0 +1,70 @@
/*
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.
*/
#ifndef ODOMETRYTHREAD_H_
#define ODOMETRYTHREAD_H_
#include <rtabmap/core/RtabmapExp.h>
#include <rtabmap/core/SensorData.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UEventsHandler.h>
namespace rtabmap {
class Odometry;
class RTABMAP_EXP OdometryThread : public UThread, public UEventsHandler {
public:
// take ownership of Odometry
OdometryThread(Odometry * odometry);
virtual ~OdometryThread();
protected:
virtual void handleEvent(UEvent * event);
private:
void mainLoopKill();
//============================================================
// MAIN LOOP
//============================================================
void mainLoop();
void addData(const SensorData & data);
void getData(SensorData & data);
private:
USemaphore _dataAdded;
UMutex _dataMutex;
SensorData _dataBuffer;
Odometry * _odometry;
bool _resetOdometry;
};
} // namespace rtabmap
#endif /* ODOMETRYTHREAD_H_ */

15
corelib/include/rtabmap/core/Parameters.h
View File

@@ -245,14 +245,12 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(FAST, Gpu, bool, false, "GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
RTABMAP_PARAM(FAST, GpuKeypointsRatio, double, 0.05, "Used with FAST GPU.");
RTABMAP_PARAM(GFTT, MaxCorners, int, 400, "");
RTABMAP_PARAM(GFTT, QualityLevel, double, 0.01, "");
RTABMAP_PARAM(GFTT, MinDistance, double, 5, "");
RTABMAP_PARAM(GFTT, BlockSize, int, 3, "");
RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false, "");
RTABMAP_PARAM(GFTT, K, double, 0.04, "");
RTABMAP_PARAM(ORB, NFeatures, int, 400, "The maximum number of features to retain.");
RTABMAP_PARAM(ORB, ScaleFactor, float, 1.2, "Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.");
RTABMAP_PARAM(ORB, NLevels, int, 1, "The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).");
RTABMAP_PARAM(ORB, EdgeThreshold, int, 31, "This is size of the border where the features are not detected. It should roughly match the patchSize parameter.");
@@ -309,7 +307,7 @@ class RTABMAP_EXP Parameters
// Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Bag-of-words 1=Optical Flow");
RTABMAP_PARAM(Odom, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
RTABMAP_PARAM(Odom, MaxFeatures, int, 0, "0 no limits.");
RTABMAP_PARAM(Odom, MaxFeatures, int, 400, "0 no limits.");
RTABMAP_PARAM(Odom, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences.");
RTABMAP_PARAM(Odom, MinInliers, int, 20, "Minimum visual word correspondences to compute geometry transform.");
RTABMAP_PARAM(Odom, Iterations, int, 30, "Maximum iterations to compute the transform from visual words.");
@@ -328,6 +326,13 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(OdomBow, NNType, int, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
RTABMAP_PARAM(OdomBow, NNDR, float, 0.8, "NNDR: nearest neighbor distance ratio.");
// Odometry Mono
RTABMAP_PARAM(OdomMono, InitMinFlow, float, 100, "Minimum optical flow required for the initialization step.");
RTABMAP_PARAM(OdomMono, InitMinTranslation, float, 0.1, "Minimum translation required for the initialization step.");
RTABMAP_PARAM(OdomMono, MinTranslation, float, 0.02, "Minimum translation to add new points to local map. On initialization, translation x 5 is used as the minimum.");
RTABMAP_PARAM(OdomMono, MaxVariance, float, 0.01, "Maximum variance to add new points to local map.");
// Odometry common stuff between BOW and Optical Flow approaches
RTABMAP_PARAM(OdomFlow, WinSize, int, 16, "Used for optical flow approach and for stereo matching. See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(OdomFlow, Iterations, int, 30, "Used for optical flow approach and for stereo matching. See cv::calcOpticalFlowPyrLK().");
@@ -346,7 +351,9 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(LccBow, InlierDistance, float, 0.02, "Maximum distance for visual word correspondences.");
RTABMAP_PARAM(LccBow, Iterations, int, 100, "Maximum iterations to compute the transform from visual words.");
RTABMAP_PARAM(LccBow, MaxDepth, float, 4.0, "Max depth of the words (0 means no limit).");
RTABMAP_PARAM(LccBow, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw).");
RTABMAP_PARAM(LccBow, Force2D, bool, false, "Force 2D transform (3Dof: x,y and yaw).");
RTABMAP_PARAM(LccBow, EpipolarGeometry, bool, false, "Use epipolar geometry to compute the loop closure transform.");
RTABMAP_PARAM(LccBow, EpipolarGeometryVar, float, 0.02, "Epipolar geometry maximum variance to accept the loop closure.");
RTABMAP_PARAM_COND(LccReextract, Activated, bool, RTABMAP_NONFREE, false, true, "Activate re-extracting features on global loop closure.");
RTABMAP_PARAM(LccReextract, NNType, int, 3, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4.");
RTABMAP_PARAM(LccReextract, NNDR, float, 0.8, "NNDR: nearest neighbor distance ratio.");

12
corelib/include/rtabmap/core/Signature.h
View File

@@ -134,10 +134,14 @@ public:
const std::multimap<int, pcl::PointXYZ> & getWords3() const {return _words3;}
const cv::Mat & getDepthCompressed() const {return _depthCompressed;}
const cv::Mat & getLaserScanCompressed() const {return _laserScanCompressed;}
float getDepthFx() const {return _fx;}
float getDepthFy() const {return _fy;}
float getDepthCx() const {return _cx;}
float getDepthCy() const {return _cy;}
RTABMAP_DEPRECATED(float getDepthFx() const, "Use getFx() instead.");
RTABMAP_DEPRECATED(float getDepthFy() const, "Use getFy() instead.");
RTABMAP_DEPRECATED(float getDepthCx() const, "Use getCx() instead.");
RTABMAP_DEPRECATED(float getDepthCy() const, "Use getCy() instead.");
float getFx() const {return _fx;}
float getFy() const {return _fy;}
float getCx() const {return _cx;}
float getCy() const {return _cy;}
const Transform & getPose() const {return _pose;}
const Transform & getLocalTransform() const {return _localTransform;}
void setDepthRaw(const cv::Mat & depth) {_depthRaw = depth;}

3
corelib/include/rtabmap/core/Statistics.h
View File

@@ -99,9 +99,8 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(TimingMem, Signature_creation, ms);
RTABMAP_STATS(TimingMem, Rehearsal, ms);
RTABMAP_STATS(TimingMem, Keypoints_detection, ms);
RTABMAP_STATS(TimingMem, Stereo_subpixel, ms);
RTABMAP_STATS(TimingMem, Subpixel, ms);
RTABMAP_STATS(TimingMem, Stereo_correspondences, ms);
RTABMAP_STATS(TimingMem, Keypoints_filtering, ms);
RTABMAP_STATS(TimingMem, Descriptors_extraction, ms);
RTABMAP_STATS(TimingMem, Keypoints_3D, ms);
RTABMAP_STATS(TimingMem, Joining_dictionary_update, ms);

23
corelib/include/rtabmap/core/util3d.h
View File

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/RtabmapExp.h"
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <list>
#include <string>
@@ -99,6 +100,23 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DStereo(
int flowIterations = 20,
double flowEps = 0.02);
std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3DMono(
const std::multimap<int, cv::KeyPoint> & kpts,
const std::multimap<int, cv::KeyPoint> & previousKpts,
float fx,
float fy,
float cx,
float cy,
const Transform & localTransform,
Transform & cameraTransform,
int pnpIterations = 100,
float pnpReprojError = 8.0f,
int pnpFlags = cv::ITERATIVE,
float ransacParam1 = 3.0f,
float ransacParam2 = 0.99f,
const std::multimap<int, pcl::PointXYZ> & refGuess3D = std::multimap<int, pcl::PointXYZ>(),
double * variance = 0);
std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
const std::list<int> & wordIds,
const std::vector<cv::KeyPoint> & keypoints);
@@ -412,6 +430,11 @@ pcl::IndicesPtr RTABMAP_EXP concatenate(
cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d);
void RTABMAP_EXP savePCDWords(
const std::string & fileName,
const std::multimap<int, pcl::PointXYZ> & words,
const Transform & transform = Transform::getIdentity());
///////////////////
// Templated PCL methods
///////////////////

8
corelib/src/CMakeLists.txt
View File

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

21
corelib/src/CameraRGBD.cpp
View File

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

8
corelib/src/DBDriver.cpp
View File

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

2
corelib/src/DBDriverSqlite3.cpp
View File

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

11
corelib/src/EpipolarGeometry.cpp
View File

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

21
corelib/src/Features2d.cpp
View File

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

325
corelib/src/Memory.cpp
View File

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

1602
corelib/src/Odometry.cpp
View File

File diff suppressed because it is too large Load Diff

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

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

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

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

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

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

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

@@ -0,0 +1,951 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Odometry.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UStl.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>
namespace rtabmap {
OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
Odometry(parameters),
flowWinSize_(Parameters::defaultOdomFlowWinSize()),
flowIterations_(Parameters::defaultOdomFlowIterations()),
flowEps_(Parameters::defaultOdomFlowEps()),
flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
stereoWinSize_(Parameters::defaultStereoWinSize()),
stereoIterations_(Parameters::defaultStereoIterations()),
stereoEps_(Parameters::defaultStereoEps()),
stereoMaxLevel_(Parameters::defaultStereoMaxLevel()),
stereoMaxSlope_(Parameters::defaultStereoMaxSlope()),
subPixWinSize_(Parameters::defaultOdomSubPixWinSize()),
subPixIterations_(Parameters::defaultOdomSubPixIterations()),
subPixEps_(Parameters::defaultOdomSubPixEps()),
refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
{
Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
Parameters::parse(parameters, Parameters::kStereoWinSize(), stereoWinSize_);
Parameters::parse(parameters, Parameters::kStereoIterations(), stereoIterations_);
Parameters::parse(parameters, Parameters::kStereoEps(), stereoEps_);
Parameters::parse(parameters, Parameters::kStereoMaxLevel(), stereoMaxLevel_);
Parameters::parse(parameters, Parameters::kStereoMaxSlope(), stereoMaxSlope_);
Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize_);
Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations_);
Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps_);
ParametersMap::const_iterator iter;
Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
if((iter=parameters.find(Parameters::kOdomFeatureType())) != parameters.end())
{
detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
}
ParametersMap customParameters;
int maxFeatures = Parameters::defaultOdomMaxFeatures();
Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
// add only feature stuff
for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string group = uSplit(iter->first, '/').front();
if(group.compare("SURF") == 0 ||
group.compare("SIFT") == 0 ||
group.compare("BRIEF") == 0 ||
group.compare("FAST") == 0 ||
group.compare("ORB") == 0 ||
group.compare("FREAK") == 0 ||
group.compare("GFTT") == 0 ||
group.compare("BRISK") == 0)
{
customParameters.insert(*iter);
}
}
feature2D_ = Feature2D::create(detectorStrategy, customParameters);
}
OdometryOpticalFlow::~OdometryOpticalFlow()
{
delete feature2D_;
}
void OdometryOpticalFlow::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
refFrame_ = cv::Mat();
refCorners_.clear();
refCorners3D_->clear();
}
// return not null transform if odometry is correctly computed
Transform OdometryOpticalFlow::computeTransform(
const SensorData & data,
OdometryInfo * info)
{
UDEBUG("");
if(info)
{
info->type = 1;
}
if(!data.rightImage().empty())
{
//stereo
return computeTransformStereo(data, info);
}
else
{
//rgbd
return computeTransformRGBD(data, info);
}
}
Transform OdometryOpticalFlow::computeTransformStereo(
const SensorData & data,
OdometryInfo * info)
{
UTimer timer;
Transform output;
double variance = 0;
int inliers = 0;
int correspondences = 0;
cv::Mat newLeftFrame;
// convert to grayscale
if(data.image().channels() > 1)
{
cv::cvtColor(data.image(), newLeftFrame, cv::COLOR_BGR2GRAY);
}
else
{
newLeftFrame = data.image().clone();
}
cv::Mat newRightFrame = data.rightImage().clone();
std::vector<cv::Point2f> newCorners;
UDEBUG("lastCorners_.size()=%d lastFrame_=%d lastRightFrame_=%d", (int)refCorners_.size(), refFrame_.empty()?0:1, refRightFrame_.empty()?0:1);
if(!refFrame_.empty() && !refRightFrame_.empty() && refCorners_.size())
{
UDEBUG("");
// Find features in the new left image
std::vector<unsigned char> status;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
refFrame_,
newLeftFrame,
refCorners_,
newCorners,
status,
err,
cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
std::vector<cv::Point2f> lastCornersKept(status.size());
std::vector<cv::Point2f> newCornersKept(status.size());
int ki = 0;
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i])
{
lastCornersKept[ki] = refCorners_[i];
newCornersKept[ki] = newCorners[i];
++ki;
}
}
lastCornersKept.resize(ki);
newCornersKept.resize(ki);
if(ki && ki >= this->getMinInliers())
{
std::vector<unsigned char> statusLast;
std::vector<float> errLast;
std::vector<cv::Point2f> lastCornersKeptRight;
UDEBUG("previous stereo disparity");
cv::calcOpticalFlowPyrLK(
refFrame_,
refRightFrame_,
lastCornersKept,
lastCornersKeptRight,
statusLast,
errLast,
cv::Size(stereoWinSize_, stereoWinSize_), stereoMaxLevel_,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, stereoIterations_, stereoEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("new stereo disparity");
std::vector<unsigned char> statusNew;
std::vector<float> errNew;
std::vector<cv::Point2f> newCornersKeptRight;
cv::calcOpticalFlowPyrLK(
newLeftFrame,
newRightFrame,
newCornersKept,
newCornersKeptRight,
statusNew,
errNew,
cv::Size(stereoWinSize_, stereoWinSize_), stereoMaxLevel_,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, stereoIterations_, stereoEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
if(this->isPnPEstimationUsed())
{
// find correspondences
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(statusLast.size());
info->newCorners.resize(statusLast.size());
}
int flowInliers = 0;
std::vector<cv::Point3f> objectPoints(statusLast.size());
std::vector<cv::Point2f> imagePoints(statusLast.size());
std::vector<pcl::PointXYZ> image3DPoints(statusLast.size());
int oi=0;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(unsigned int i=0; i<statusLast.size(); ++i)
{
if(statusLast[i])
{
float lastDisparity = lastCornersKept[i].x - lastCornersKeptRight[i].x;
float lastSlope = fabs((lastCornersKept[i].y-lastCornersKeptRight[i].y) / (lastCornersKept[i].x-lastCornersKeptRight[i].x));
float newDisparity = newCornersKept[i].x - newCornersKeptRight[i].x;
float newSlope = fabs((newCornersKept[i].y-newCornersKeptRight[i].y) / (newCornersKept[i].x-newCornersKeptRight[i].x));
if(lastDisparity > 0.0f && lastSlope < stereoMaxSlope_)
{
pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D(
lastCornersKept[i],
lastDisparity,
data.cx(), data.cy(), data.fx(), data.baseline());
if(pcl::isFinite(lastPt3D) &&
(this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth())))
{
//Add 3D correspondences!
lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
objectPoints[oi].x = lastPt3D.x;
objectPoints[oi].y = lastPt3D.y;
objectPoints[oi].z = lastPt3D.z;
imagePoints[oi] = newCornersKept.at(i);
// new 3D points, used to compute variance
image3DPoints[oi] = pcl::PointXYZ(bad_point, bad_point, bad_point);
if(newDisparity > 0.0f && newSlope < stereoMaxSlope_)
{
pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D(
newCornersKept[i],
newDisparity,
data.cx(), data.cy(), data.fx(), data.baseline());
if(pcl::isFinite(newPt3D) &&
(this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth())))
{
image3DPoints[oi] = util3d::transformPoint(newPt3D, data.localTransform());
}
}
if(this->isInfoDataFilled() && info)
{
info->refCorners[oi] = lastCornersKept[i];
info->newCorners[oi] = newCornersKept[i];
}
++oi;
}
}
++flowInliers;
}
}
objectPoints.resize(oi);
imagePoints.resize(oi);
image3DPoints.resize(oi);
UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(oi);
info->newCorners.resize(oi);
}
correspondences = oi;
if(correspondences >= this->getMinInliers())
{
//PnPRansac
cv::Mat K = (cv::Mat_<double>(3,3) <<
data.fx(), 0, data.cx(),
0, data.fx(), data.cy(),
0, 0, 1);
Transform guess = (data.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
std::vector<int> inliersV;
cv::solvePnPRansac(objectPoints,
imagePoints,
K,
cv::Mat(),
rvec,
tvec,
true,
this->getIterations(),
this->getPnPReprojError(),
0,
inliersV,
this->getPnPFlags());
inliers = (int)inliersV.size();
if((int)inliersV.size() >= this->getMinInliers())
{
cv::Rodrigues(rvec, R);
Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
// make it incremental
output = (data.localTransform() * pnp).inverse();
UDEBUG("Odom transform = %s", output.prettyPrint().c_str());
// compute variance (like in PCL computeVariance() method of sac_model.h)
std::vector<float> errorSqrdDists(inliersV.size());
int ii=0;
for(unsigned int i=0; i<inliersV.size(); ++i)
{
pcl::PointXYZ & newPt = image3DPoints[inliersV[i]];
if(pcl::isFinite(newPt))
{
newPt = util3d::transformPoint(newPt, output);
const cv::Point3f & objPt = objectPoints[inliersV[i]];
errorSqrdDists[ii++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
}
}
errorSqrdDists.resize(ii);
if(errorSqrdDists.size())
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
variance = 2.1981 * median_error_sqr;
}
}
else
{
UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
}
if(this->isInfoDataFilled() && info)
{
info->cornerInliers = inliersV;
}
}
else
{
UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
}
}
else
{
UDEBUG("Getting correspondences begin");
// Get 3D correspondences
pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
correspondencesLast->resize(statusLast.size());
correspondencesNew->resize(statusLast.size());
int oi = 0;
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(statusLast.size());
info->newCorners.resize(statusLast.size());
}
for(unsigned int i=0; i<statusLast.size(); ++i)
{
if(statusLast[i] && statusNew[i])
{
float lastDisparity = lastCornersKept[i].x - lastCornersKeptRight[i].x;
float newDisparity = newCornersKept[i].x - newCornersKeptRight[i].x;
float lastSlope = fabs((lastCornersKept[i].y-lastCornersKeptRight[i].y) / (lastCornersKept[i].x-lastCornersKeptRight[i].x));
float newSlope = fabs((newCornersKept[i].y-newCornersKeptRight[i].y) / (newCornersKept[i].x-newCornersKeptRight[i].x));
if(lastDisparity > 0.0f && newDisparity > 0.0f &&
lastSlope < stereoMaxSlope_ && newSlope < stereoMaxSlope_)
{
pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D(
lastCornersKept[i],
lastDisparity,
data.cx(), data.cy(), data.fx(), data.baseline());
pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D(
newCornersKept[i],
newDisparity,
data.cx(), data.cy(), data.fx(), data.baseline());
if(pcl::isFinite(lastPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth())) &&
pcl::isFinite(newPt3D) && (this->getMaxDepth() == 0.0f || uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth())))
{
//Add 3D correspondences!
lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
newPt3D = util3d::transformPoint(newPt3D, data.localTransform());
correspondencesLast->at(oi) = lastPt3D;
correspondencesNew->at(oi) = newPt3D;
if(this->isInfoDataFilled() && info)
{
info->refCorners[oi] = lastCornersKept[i];
info->newCorners[oi] = newCornersKept[i];
}
++oi;
}
}
}
}// end loop
correspondencesLast->resize(oi);
correspondencesNew->resize(oi);
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(oi);
info->newCorners.resize(oi);
}
correspondences = oi;
refCorners3D_ = correspondencesNew;
UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)statusLast.size());
if(correspondences >= this->getMinInliers())
{
std::vector<int> inliersV;
UTimer timerRANSAC;
Transform t = util3d::transformFromXYZCorrespondences(
correspondencesNew,
correspondencesLast,
this->getInlierDistance(),
this->getIterations(),
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
&inliersV,
&variance);
UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
inliers = (int)inliersV.size();
if(!t.isNull() && inliers >= this->getMinInliers())
{
output = t;
}
else
{
UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
}
if(this->isInfoDataFilled() && info)
{
info->cornerInliers = inliersV;
}
}
else
{
UWARN("Not enough correspondences (%d)", correspondences);
}
}
}
}
else
{
//return Identity
output = Transform::getIdentity();
}
newCorners.clear();
if(!output.isNull())
{
// Copy or generate new keypoints
if(data.keypoints().size())
{
newCorners.resize(data.keypoints().size());
for(unsigned int i=0; i<data.keypoints().size(); ++i)
{
newCorners[i] = data.keypoints().at(i).pt;
}
}
else
{
// generate kpts
std::vector<cv::KeyPoint> newKtps;
cv::Rect roi = Feature2D::computeRoi(newLeftFrame, this->getRoiRatios());
newKtps = feature2D_->generateKeypoints(newLeftFrame, roi);
if(newKtps.size())
{
cv::KeyPoint::convert(newKtps, newCorners);
if(subPixWinSize_ > 0 && subPixIterations_ > 0)
{
UDEBUG("cv::cornerSubPix() begin");
cv::cornerSubPix(newLeftFrame, newCorners,
cv::Size( subPixWinSize_, subPixWinSize_ ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
UDEBUG("cv::cornerSubPix() end");
}
}
}
if((int)newCorners.size() > this->getMinInliers())
{
refFrame_ = newLeftFrame;
refRightFrame_ = newRightFrame;
refCorners_ = newCorners;
}
else
{
UWARN("Too low 2D corners (%d), ignoring new frame...",
(int)newCorners.size());
output.setNull();
}
}
if(info)
{
info->type = 1;
info->variance = variance;
info->inliers = inliers;
info->features = (int)newCorners.size();
info->matches = correspondences;
}
UINFO("Odom update time = %fs lost=%s inliers=%d/%d, new corners=%d, transform accepted=%s",
timer.elapsed(),
output.isNull()?"true":"false",
inliers,
correspondences,
(int)newCorners.size(),
!output.isNull()?"true":"false");
return output;
}
Transform OdometryOpticalFlow::computeTransformRGBD(
const SensorData & data,
OdometryInfo * info)
{
UTimer timer;
Transform output;
double variance = 0;
int inliers = 0;
int correspondences = 0;
cv::Mat newFrame;
// convert to grayscale
if(data.image().channels() > 1)
{
cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY);
}
else
{
newFrame = data.image().clone();
}
std::vector<cv::Point2f> newCorners;
if(!refFrame_.empty() &&
(int)refCorners_.size() >= this->getMinInliers() &&
(int)refCorners3D_->size() >= this->getMinInliers())
{
std::vector<unsigned char> status;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
refFrame_,
newFrame,
refCorners_,
newCorners,
status,
err,
cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
if(this->isPnPEstimationUsed())
{
// find correspondences
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(refCorners_.size());
info->newCorners.resize(refCorners_.size());
}
UASSERT(refCorners_.size() == refCorners3D_->size());
UDEBUG("lastCorners3D_ = %d", refCorners3D_->size());
int flowInliers = 0;
std::vector<cv::Point3f> objectPoints(refCorners_.size());
std::vector<cv::Point2f> imagePoints(refCorners_.size());
std::vector<pcl::PointXYZ> image3DPoints(refCorners_.size());
int oi=0;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i])
{
if(pcl::isFinite(refCorners3D_->at(i)))
{
objectPoints[oi].x = refCorners3D_->at(i).x;
objectPoints[oi].y = refCorners3D_->at(i).y;
objectPoints[oi].z = refCorners3D_->at(i).z;
imagePoints[oi] = newCorners.at(i);
// new 3D points, used to compute variance
image3DPoints[oi] = pcl::PointXYZ(bad_point, bad_point, bad_point);
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true);
if(pcl::isFinite(pt) &&
(this->getMaxDepth() == 0.0f || (
uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
{
image3DPoints[oi] = util3d::transformPoint(pt, data.localTransform());
}
}
if(this->isInfoDataFilled() && info)
{
info->refCorners[oi] = refCorners_[i];
info->newCorners[oi] = newCorners[i];
}
++oi;
}
++flowInliers;
}
}
objectPoints.resize(oi);
imagePoints.resize(oi);
image3DPoints.resize(oi);
UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(oi);
info->newCorners.resize(oi);
}
correspondences = oi;
if(correspondences >= this->getMinInliers())
{
//PnPRansac
cv::Mat K = (cv::Mat_<double>(3,3) <<
data.fx(), 0, data.cx(),
0, data.fy(), data.cy(),
0, 0, 1);
Transform guess = (data.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
std::vector<int> inliersV;
cv::solvePnPRansac(objectPoints,
imagePoints,
K,
cv::Mat(),
rvec,
tvec,
true,
this->getIterations(),
this->getPnPReprojError(),
0,
inliersV,
this->getPnPFlags());
inliers = (int)inliersV.size();
if((int)inliersV.size() >= this->getMinInliers())
{
cv::Rodrigues(rvec, R);
Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
// make it incremental
output = (data.localTransform() * pnp).inverse();
UDEBUG("Odom transform = %s", output.prettyPrint().c_str());
// compute variance (like in PCL computeVariance() method of sac_model.h)
std::vector<float> errorSqrdDists(inliersV.size());
int ii=0;
for(unsigned int i=0; i<inliersV.size(); ++i)
{
pcl::PointXYZ & newPt = image3DPoints[inliersV[i]];
if(pcl::isFinite(newPt))
{
newPt = util3d::transformPoint(newPt, output);
const cv::Point3f & objPt = objectPoints[inliersV[i]];
errorSqrdDists[ii++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
}
}
errorSqrdDists.resize(ii);
if(errorSqrdDists.size())
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
variance = 2.1981 * median_error_sqr;
}
}
else
{
UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
}
if(this->isInfoDataFilled() && info)
{
info->cornerInliers = inliersV;
}
}
else
{
UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
}
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
correspondencesLast->resize(refCorners_.size());
correspondencesNew->resize(refCorners_.size());
int oi=0;
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(refCorners_.size());
info->newCorners.resize(refCorners_.size());
}
UASSERT(refCorners_.size() == refCorners3D_->size());
UDEBUG("lastCorners3D_ = %d", refCorners3D_->size());
int flowInliers = 0;
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] && pcl::isFinite(refCorners3D_->at(i)) &&
uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true);
if(pcl::isFinite(pt) &&
(this->getMaxDepth() == 0.0f || (
uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
{
pt = util3d::transformPoint(pt, data.localTransform());
correspondencesLast->at(oi) = refCorners3D_->at(i);
correspondencesNew->at(oi) = pt;
if(this->isInfoDataFilled() && info)
{
info->refCorners[oi] = refCorners_[i];
info->newCorners[oi] = newCorners[i];
}
++oi;
}
++flowInliers;
}
else if(status[i])
{
++flowInliers;
}
}
UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
if(this->isInfoDataFilled() && info)
{
info->refCorners.resize(oi);
info->newCorners.resize(oi);
}
correspondencesLast->resize(oi);
correspondencesNew->resize(oi);
correspondences = oi;
if(correspondences >= this->getMinInliers())
{
std::vector<int> inliersV;
UTimer timerRANSAC;
output = util3d::transformFromXYZCorrespondences(
correspondencesNew,
correspondencesLast,
this->getInlierDistance(),
this->getIterations(),
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
&inliersV,
&variance);
UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
inliers = (int)inliersV.size();
if(inliers < this->getMinInliers())
{
output.setNull();
UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
}
if(this->isInfoDataFilled() && info)
{
info->cornerInliers = inliersV;
}
}
else
{
UWARN("Not enough correspondences (%d)", correspondences);
}
}
}
else
{
//return Identity
output = Transform::getIdentity();
}
newCorners.clear();
if(!output.isNull())
{
// Copy or generate new keypoints
if(data.keypoints().size())
{
newCorners.resize(data.keypoints().size());
for(unsigned int i=0; i<data.keypoints().size(); ++i)
{
newCorners[i] = data.keypoints().at(i).pt;
}
}
else
{
// generate kpts
std::vector<cv::KeyPoint> newKtps;
cv::Rect roi = Feature2D::computeRoi(newFrame, this->getRoiRatios());
newKtps = feature2D_->generateKeypoints(newFrame, roi);
Feature2D::filterKeypointsByDepth(newKtps, data.depth(), this->getMaxDepth());
if(newKtps.size())
{
cv::KeyPoint::convert(newKtps, newCorners);
if(subPixWinSize_ > 0 && subPixIterations_ > 0)
{
cv::cornerSubPix(newFrame, newCorners,
cv::Size( subPixWinSize_, subPixWinSize_ ),
cv::Size( -1, -1 ),
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
}
}
}
if((int)newCorners.size() > this->getMinInliers())
{
// get 3D corners for the extracted 2D corners (not the ones refined by Optical Flow)
pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
newCorners3D->resize(newCorners.size());
std::vector<cv::Point2f> newCornersFiltered(newCorners.size());
int oi=0;
for(unsigned int i=0; i<newCorners.size(); ++i)
{
if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)) &&
uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)))
{
pcl::PointXYZ pt = util3d::projectDepthTo3D(data.depth(), newCorners[i].x, newCorners[i].y,
data.cx(), data.cy(), data.fx(), data.fy(), true);
if(pcl::isFinite(pt) &&
(this->getMaxDepth() == 0.0f || (
uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))))
{
pt = util3d::transformPoint(pt, data.localTransform());
newCorners3D->at(oi) = pt;
newCornersFiltered[oi] = newCorners[i];
++oi;
}
}
}
newCornersFiltered.resize(oi);
newCorners3D->resize(oi);
if((int)newCornersFiltered.size() > this->getMinInliers())
{
refFrame_ = newFrame;
refCorners_ = newCornersFiltered;
refCorners3D_ = newCorners3D;
}
else
{
UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...",
(int)newCornersFiltered.size(), (int)refCorners3D_->size(), this->getMinInliers());
output.setNull();
}
}
else
{
UWARN("Too low 2D corners (%d), ignoring new frame...",
(int)newCorners.size());
output.setNull();
}
}
if(info)
{
info->type = 1;
info->variance = variance;
info->inliers = inliers;
info->features = (int)newCorners.size();
info->matches = correspondences;
}
UINFO("Odom update time = %fs lost=%s inliers=%d/%d, variance=%f, new corners=%d",
timer.elapsed(),
output.isNull()?"true":"false",
inliers,
correspondences,
variance,
(int)newCorners.size());
return output;
}
} // namespace rtabmap

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

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

4
corelib/src/SensorData.cpp
View File

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

5
corelib/src/Signature.cpp
View File

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

300
corelib/src/util3d.cpp
View File

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