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

Refactored Odometry class:

Browse Source 
-new odometry parameters
-new Optical flow strategy
-Stereo data support 
Refactored SensorData class to support stereo images
Updated default parameters (mostly odometry ones)
util3d: new methods to handle/reconstruct 3D clouds from disparity image / stereo images
PreferencesDialog: added Odometry/BOW and Odometry/OpticalFLow panels.
DatabaseViewer: fixed a crash when database is empty. Added cloud reconstruction of stereo images if saved in database
Camera: added 1 second delay to avoid dark images at the starting


git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1849 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-10-13 19:10:22 +00:00
parent a3f7415821
commit 8b67633b34
25 changed files with 2108 additions and 498 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -48,29 +48,6 @@ namespace gpu {
namespace rtabmap {
void RTABMAP_EXP filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
float maxDepth);
void RTABMAP_EXP filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors,
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
float maxDepth);
void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
void RTABMAP_EXP limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
// Feature2D
class RTABMAP_EXP Feature2D {
public:
@@ -84,6 +61,23 @@ public:
kFeatureGfttBrief=6,
kFeatureBrisk=7};
static Feature2D * create(Feature2D::Type & type, const ParametersMap & parameters);
static void filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
float maxDepth);
static void filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors,
const cv::Mat & depth,
float maxDepth);
static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
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);
public:
virtual ~Feature2D() {}

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

@@ -168,10 +168,6 @@ public:
void extractKeypointsAndDescriptors(
const cv::Mat & image,
const cv::Mat & depth,
float fx,
float fy,
float cx,
float cy,
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors) const;

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

@@ -50,6 +50,8 @@ class UTimer;
namespace rtabmap {
class Feature2D;
class RTABMAP_EXP Odometry
{
public:
@@ -62,29 +64,31 @@ 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;}
int getIterations() const {return _iterations;}
float getWordsRatio() const {return _wordsRatio;}
int getRefineIterations() const {return _refineIterations;}
float getFeaturesRatio() const {return _featuresRatio;}
float getMaxDepth() const {return _maxDepth;}
float geLinearUpdate() const {return _linearUpdate;}
float getAngularUpdate() const {return _angularUpdate;}
int getLocalHistoryMaxSize() const {return _localHistoryMaxSize;}
private:
virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0) = 0;
private:
int _maxFeatures;
std::string _roiRatios;
int _minInliers;
float _inlierDistance;
int _iterations;
float _wordsRatio;
int _refineIterations;
float _featuresRatio;
float _maxDepth;
float _linearUpdate;
float _angularUpdate;
int _resetCountdown;
int _localHistoryMaxSize;
Transform _pose;
int _resetCurrentCount;
@@ -108,10 +112,51 @@ private:
virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
private:
//Parameters
int _localHistoryMaxSize;
Memory * _memory;
std::multimap<int, pcl::PointXYZ> localMap_;
};
class RTABMAP_EXP OdometryOpticalFlow : public Odometry
{
public:
OdometryOpticalFlow(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
virtual ~OdometryOpticalFlow();
virtual void reset();
const cv::Mat & getLastFrame() const {return lastFrame_;}
const std::vector<cv::Point2f> & getLastCorners() const {return lastCorners_;}
const pcl::PointCloud<pcl::PointXYZ>::Ptr & getLastCorners3D() const {return lastCorners3D_;}
cv::Mat imgMatches_;
private:
virtual Transform computeTransform(const SensorData & image, int * quality = 0, int * features = 0, int * localMapSize = 0);
Transform computeTransformStereo(const SensorData & image, int * quality, int * features);
Transform computeTransformRGBD(const SensorData & image, int * quality, int * features);
private:
//Parameters:
int flowWinSize_;
int flowIterations_;
double flowEps_;
int flowMaxLevel_;
int subPixWinSize_;
int subPixIterations_;
double subPixEps_;
Feature2D * feature2D_;
cv::Mat lastFrame_;
cv::Mat lastRightFrame_;
std::vector<cv::Point2f> lastCorners_;
pcl::PointCloud<pcl::PointXYZ>::Ptr lastCorners3D_;
Transform savedLastRefFrameTransform_;
};
class RTABMAP_EXP OdometryICP : public Odometry
{
public:

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

@@ -204,7 +204,7 @@ 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, 1000, "");
RTABMAP_PARAM(GFTT, MaxCorners, int, 400, "");
RTABMAP_PARAM(GFTT, QualityLevel, double, 0.01, "");
RTABMAP_PARAM(GFTT, MinDistance, double, 1, "");
RTABMAP_PARAM(GFTT, BlockSize, int, 3, "");
@@ -247,29 +247,43 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, ToroIterations, int, 100, "TORO graph optimization iterations");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, true, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
// Local loop closure detection
RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, true, "Detection over all locations in STM.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, false, "Detection over all locations in STM.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace, bool, false, "Detection over locations (in Working Memory or STM) near in space.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionRadius, float, 15, "Maximum radius for space detection.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionNeighbors, int, 20, "Maximum nearest neighbor.");
RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxDiffID, int, 0, "Maximum ID difference between the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.")
// Odometry
RTABMAP_PARAM(Odom, Type, int, 0, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK.");
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, LinearUpdate, float, 0.0, "Min linear displacement to update odometry.");
RTABMAP_PARAM(Odom, AngularUpdate, float, 0.0, "Min angular displacement to update odometry.");
RTABMAP_PARAM(Odom, MaxWords, int, 0, "0 no limits.");
RTABMAP_PARAM(Odom, MaxFeatures, int, 0, "0 no limits.");
RTABMAP_PARAM(Odom, InlierDistance, float, 0.01, "Maximum distance for visual word correspondences.");
RTABMAP_PARAM(Odom, MinInliers, int, 10, "Minimum visual word correspondences to compute geometry transform.");
RTABMAP_PARAM(Odom, Iterations, int, 100, "Maximum iterations to compute the transform from visual words.");
RTABMAP_PARAM(Odom, MaxDepth, float, 5.0, "Max depth of the words (0 means no limit).");
RTABMAP_PARAM(Odom, WordsRatio, float, 0.5, "Minmum ratio of keypoints between the current image and the last image to compute odometry.");
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.");
RTABMAP_PARAM(Odom, MaxDepth, float, 4.0, "Max depth of the words (0 means no limit).");
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
RTABMAP_PARAM(Odom, LocalHistory, int, 0, "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
RTABMAP_PARAM(Odom, NearestNeighbor, int, 1, "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
RTABMAP_PARAM(Odom, NNDR, float, 0.7, "NNDR: nearest neighbor distance ratio.");
RTABMAP_PARAM_STR(Odom, RoiRatios, "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
RTABMAP_PARAM(Odom, RefineIterations, int, 10, "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
RTABMAP_PARAM(Odom, FeaturesRatio, float, 0.5, "Minimum ratio of keypoints between the current image and the last image to compute odometry.");
// Odometry Bag-of-words
RTABMAP_PARAM(OdomBow, LocalHistorySize, int, 1000, "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
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 Optical Flow
RTABMAP_PARAM(OdomFlow, WinSize, int, 9, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(OdomFlow, Iterations, int, 20, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(OdomFlow, Eps, double, 0.02, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(OdomFlow, MaxLevel, int, 4, "See cv::calcOpticalFlowPyrLK().");
RTABMAP_PARAM(OdomFlow, SubPixWinSize, int, 3, "See cv::cornerSubPix().");
RTABMAP_PARAM(OdomFlow, SubPixIterations, int, 0, "See cv::cornerSubPix(). 0 disables sub pixel refining.");
RTABMAP_PARAM(OdomFlow, SubPixEps, double, 0.02, "See cv::cornerSubPix().");
// Loop closure constraint
RTABMAP_PARAM(LccIcp, Type, int, 0, "0=No ICP, 1=ICP 3D, 2=ICP 2D");

28
corelib/include/rtabmap/core/SensorData.h
View File

@@ -47,9 +47,9 @@ public:
// Metric constructor
SensorData(const cv::Mat & image,
const cv::Mat & depth,
const cv::Mat & depthOrRightImage,
float fx,
float fy,
float fyOrBaseline,
float cx,
float cy,
const Transform & pose,
@@ -58,10 +58,10 @@ public:
// Metric constructor + 2d depth
SensorData(const cv::Mat & image,
const cv::Mat & depth,
const cv::Mat & depthOrRightImage,
const cv::Mat & depth2d,
float fx,
float fy,
float fyOrBaseline,
float cx,
float cy,
const Transform & pose,
@@ -78,14 +78,18 @@ public:
const cv::Mat & image() const {return _image;}
int id() const {return _id;};
bool isMetric() const {return !_depth.empty() || _fx != 0.0f || _fy != 0.0f || !_pose.isNull();}
bool isMetric() const {return !_depthOrRightImage.empty() || _fx != 0.0f || _fyOrBaseline != 0.0f || !_pose.isNull();}
void setPose(const Transform & pose) {_pose = pose;}
const cv::Mat & depth() const {return _depth;}
cv::Mat depth() const {return (_depthOrRightImage.type()==CV_32FC1 || _depthOrRightImage.type()==CV_16UC1)?_depthOrRightImage:cv::Mat();}
cv::Mat rightImage() const {return _depthOrRightImage.type()==CV_8UC1?_depthOrRightImage:cv::Mat();}
const cv::Mat & depthOrRightImage() const {return _depthOrRightImage;}
const cv::Mat & depth2d() const {return _depth2d;}
float depthFx() const {return _fx;}
float depthFy() const {return _fy;}
float depthCx() const {return _cx;}
float depthCy() const {return _cy;}
float fx() const {return _fx;}
float fy() const {return (_depthOrRightImage.type()==CV_32FC1 || _depthOrRightImage.type()==CV_16UC1)?_fyOrBaseline:0;}
float cx() const {return _cx;}
float cy() const {return _cy;}
float baseline() const {return _depthOrRightImage.type()==CV_8UC1?_fyOrBaseline:0;}
float fyOrBaseline() const {return _fyOrBaseline;}
const Transform & pose() const {return _pose;}
const Transform & localTransform() const {return _localTransform;}
@@ -102,10 +106,10 @@ private:
int _id;
// Metric stuff
cv::Mat _depth;
cv::Mat _depthOrRightImage;
cv::Mat _depth2d;
float _fx;
float _fy;
float _fyOrBaseline;
float _cx;
float _cy;
Transform _pose;

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

@@ -186,6 +186,26 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
float fx, float fy,
int decimation = 1);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDisparityRGB(
const cv::Mat & imageRgb,
const cv::Mat & imageDisparity,
float cx, float cy,
float fx, float baseline,
int decimation);
cv::Mat RTABMAP_EXP disparityFromStereoImages(
const cv::Mat & leftImage,
const cv::Mat & rightImage);
pcl::PointXYZ RTABMAP_EXP projectDisparityTo3d(
const cv::Point2f & pt,
float disparity,
float cx, float cy, float fx, float baseline);
cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
float cx, float cy, float fx, float baseline,
int type = CV_32FC1);
cv::Mat RTABMAP_EXP depth2DFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP depth2DToPointCloud(const cv::Mat & depth2D);
@@ -242,7 +262,10 @@ Transform RTABMAP_EXP transformFromXYZCorrespondences(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
double inlierThreshold = 0.02,
int iterations = 100,
int * inliers = 0);
bool refineModel = false,
double refineModelSigma = 3.0,
int refineModelIterations = 10,
std::vector<int> * inliers = 0);
Transform RTABMAP_EXP icp(
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,